Merge pull request #4048 from priority-kew/aliengo_reduced_meshes

Import Aliengo URDF

.github/workflows/cmake.yml

@@ -0,0 +1,38 @@
+name: CMake
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    runs-on: ubuntu-latest
+
+    steps:
+    - uses: actions/checkout@v2
+
+    - name: Configure CMake
+      # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.
+      # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type
+      run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+    - name: Build
+      # Build your program with the given configuration
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Test
+      working-directory: ${{github.workspace}}/build
+      # Execute tests defined by the CMake configuration.  
+      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
+      run: ctest -C ${{env.BUILD_TYPE}}
+      

.travis.yml

@@ -1,51 +0,0 @@
-language: cpp
-matrix:
-  include:
-    - os: linux
-      compiler: gcc
-      env:
-        - BUILD_NAME=TRUSTY_GCC
-        - SUDO=sudo
-    - os: linux
-      compiler: clang
-      env:
-        - BUILD_NAME=TRUSTY_CLANG
-        - SUDO=sudo
-    - os: linux
-      compiler: clang
-      env:
-        - BUILD_NAME=XENIAL_CLANG
-        - DOCKER_FILE="ubuntu-xenial"
-      services: docker
-    - os: linux
-      compiler: gcc
-      env:
-        - BUILD_NAME=BIONIC_GCC
-        - DOCKER_FILE="ubuntu-bionic"
-      services: docker
-    - os: linux
-      compiler: clang
-      env:
-        - BUILD_NAME=BIONIC_CLANG
-        - DOCKER_FILE="ubuntu-bionic"
-      services: docker
-    - os: osx
-      compiler: gcc
-      env:
-        - BUILD_NAME=OSX_GCC
-    - os: osx
-      compiler: clang
-      env:
-        - COMPILER=clang++
-        - BUILD_NAME=OSX_CLANG
-before_install:
-  - if [ -n "$DOCKER_FILE" ]; then
-      docker build -t "$DOCKER_FILE" -f ".ci/docker/$DOCKER_FILE" .;
-      docker run -itd -v $TRAVIS_BUILD_DIR:$TRAVIS_BUILD_DIR --env-file .ci/docker/env.list --name bullet-docker "$DOCKER_FILE";
-    fi
-script:
-  - if [ -n "$DOCKER_FILE" ]; then
-      docker exec bullet-docker /bin/sh -c "cd $TRAVIS_BUILD_DIR && ./.ci/script.sh";
-    else
-      '.ci/script.sh';
-    fi

CMakeLists.txt

@@ -28,6 +28,9 @@ SET(CLAMP_VELOCITIES "0" CACHE STRING "Clamp rigid bodies' velocity to this valu
 OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON)
 OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF)
 OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" ON)
+OPTION(USE_OPENVR "Use OpenVR for virtual reality" OFF)
+
+OPTION(ENABLE_VHACD "Use VHACD in BulletRobotics and pybullet" ON)
 
 OPTION(BULLET2_MULTITHREADING "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF)
 IF (BULLET2_MULTITHREADING)
@@ -251,7 +254,8 @@ IF(BULLET2_MULTITHREADING)
 		SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 	ENDIF (NOT MSVC)
 	IF (NOT WIN32)
-		LINK_LIBRARIES( pthread )
+		FIND_PACKAGE(Threads)
+		LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} )
 	ENDIF (NOT WIN32)
 ENDIF (BULLET2_MULTITHREADING)
 

Extras/BulletRobotics/CMakeLists.txt

@@ -7,6 +7,9 @@ INCLUDE_DIRECTORIES(
 		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
 		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
 )
+ADD_DEFINITIONS(-DSTATIC_LINK_SPD_PLUGIN)
+
+
 
 SET(BulletRobotics_INCLUDES
   ../../examples/CommonInterfaces/Common2dCanvasInterface.h
@@ -94,70 +97,103 @@ SET(BulletRobotics_INCLUDES
 )
 
 SET(BulletRobotics_SRCS ${BulletRobotics_INCLUDES}
-    ../../examples/OpenGLWindow/SimpleCamera.cpp
+	../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp
+	../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h
+	../../examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp
+	../../examples/SharedMemory/plugins/stablePDPlugin/Shape.h
+	../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp
+	../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h
+	../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp
+	../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h
+	../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp
+	../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h
+	../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp
+	../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.h
+	../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp
+	../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h
+	
+	../../examples/OpenGLWindow/SimpleCamera.cpp
 
-		../../examples/TinyRenderer/geometry.cpp
-		../../examples/TinyRenderer/model.cpp
-		../../examples/TinyRenderer/tgaimage.cpp
-		../../examples/TinyRenderer/our_gl.cpp
-		../../examples/TinyRenderer/TinyRenderer.cpp
+	../../examples/TinyRenderer/geometry.cpp
+	../../examples/TinyRenderer/model.cpp
+	../../examples/TinyRenderer/tgaimage.cpp
+	../../examples/TinyRenderer/our_gl.cpp
+	../../examples/TinyRenderer/TinyRenderer.cpp
 
-    ../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
-		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
-		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp
-		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
-		../../examples/SharedMemory/IKTrajectoryHelper.cpp
-		../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
-		../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
-		../../examples/SharedMemory/InProcessMemory.cpp
-		../../examples/SharedMemory/PhysicsClient.cpp
-		../../examples/SharedMemory/PhysicsServer.cpp
-		../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
-		../../examples/SharedMemory/PhysicsDirect.cpp
-		../../examples/SharedMemory/PhysicsDirectC_API.cpp
-		../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
-		../../examples/SharedMemory/b3PluginManager.cpp
-		../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
-		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
-		../../examples/SharedMemory/PhysicsClientC_API.cpp
-		../../examples/SharedMemory/Win32SharedMemory.cpp
-		../../examples/SharedMemory/PosixSharedMemory.cpp
+	../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
+	../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
+	../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp
+	../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
+	../../examples/SharedMemory/IKTrajectoryHelper.cpp
+	../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
+	../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
+	../../examples/SharedMemory/InProcessMemory.cpp
+	../../examples/SharedMemory/PhysicsClient.cpp
+	../../examples/SharedMemory/PhysicsServer.cpp
+	../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
+	../../examples/SharedMemory/PhysicsDirect.cpp
+	../../examples/SharedMemory/PhysicsDirectC_API.cpp
+	../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+	../../examples/SharedMemory/b3PluginManager.cpp
+	../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
+	../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
+	../../examples/SharedMemory/PhysicsClientC_API.cpp
+	../../examples/SharedMemory/Win32SharedMemory.cpp
+	../../examples/SharedMemory/PosixSharedMemory.cpp
 
-		../../examples/Utils/b3ResourcePath.cpp
-		../../examples/Utils/RobotLoggingUtil.cpp
-		../../examples/Utils/b3Clock.cpp
-		../../examples/Utils/b3ResourcePath.cpp
-		../../examples/Utils/ChromeTraceUtil.cpp
+	../../examples/Utils/b3ResourcePath.cpp
+	../../examples/Utils/RobotLoggingUtil.cpp
+	../../examples/Utils/b3Clock.cpp
+	../../examples/Utils/b3ResourcePath.cpp
+	../../examples/Utils/ChromeTraceUtil.cpp
 
-		../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
-		../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
-		../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
-		../../examples/ThirdPartyLibs/BussIK/Jacobian.cpp
-		../../examples/ThirdPartyLibs/BussIK/LinearR2.cpp
-		../../examples/ThirdPartyLibs/BussIK/LinearR3.cpp
-		../../examples/ThirdPartyLibs/BussIK/LinearR4.cpp
-		../../examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
-		../../examples/ThirdPartyLibs/BussIK/Misc.cpp
-		../../examples/ThirdPartyLibs/BussIK/Node.cpp
-		../../examples/ThirdPartyLibs/BussIK/Tree.cpp
-		../../examples/ThirdPartyLibs/BussIK/VectorRn.cpp
+	../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
+	../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+	../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
+	../../examples/ThirdPartyLibs/BussIK/Jacobian.cpp
+	../../examples/ThirdPartyLibs/BussIK/LinearR2.cpp
+	../../examples/ThirdPartyLibs/BussIK/LinearR3.cpp
+	../../examples/ThirdPartyLibs/BussIK/LinearR4.cpp
+	../../examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp
+	../../examples/ThirdPartyLibs/BussIK/Misc.cpp
+	../../examples/ThirdPartyLibs/BussIK/Node.cpp
+	../../examples/ThirdPartyLibs/BussIK/Tree.cpp
+	../../examples/ThirdPartyLibs/BussIK/VectorRn.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
 
-		../../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
 )
 
+IF(ENABLE_VHACD)
+ADD_DEFINITIONS(-DBT_ENABLE_VHACD)
+SET(BulletRobotics_SRCS ${BulletRobotics_SRCS}
+  ../../Extras/VHACD/test/src/main_vhacd.cpp
+  ../../Extras/VHACD/src/VHACD.cpp
+  ../../Extras/VHACD/src/vhacdICHull.cpp
+  ../../Extras/VHACD/src/vhacdManifoldMesh.cpp
+  ../../Extras/VHACD/src/vhacdMesh.cpp
+  ../../Extras/VHACD/src/vhacdVolume.cpp
+)
+INCLUDE_DIRECTORIES( 
+  ../../Extras/VHACD/inc
+  ../../Extras/VHACD/public
+)
+ENDIF(ENABLE_VHACD)
+
 IF(BUILD_CLSOCKET)
  ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET)
 ENDIF(BUILD_CLSOCKET)
@@ -180,7 +216,8 @@ ELSE(WIN32)
 	ENDIF(BUILD_CLSOCKET)
 		
 	IF(NOT APPLE)
-		LINK_LIBRARIES(  pthread ${DL} )
+		FIND_PACKAGE(Threads)
+		LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
 	ENDIF(NOT APPLE)
 ENDIF(WIN32)
 

Extras/BulletRoboticsGUI/CMakeLists.txt

@@ -94,21 +94,6 @@ SET(BulletRoboticsGUI_INCLUDES
 )
 
 SET(BulletRoboticsGUI_SRCS ${BulletRoboticsGUI_INCLUDES}
-
-	../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp
-  ../../examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h
-  ../../examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp
-  ../../examples/SharedMemory/plugins/stablePDPlugin/Shape.h
-  ../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp
-  ../../examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h
-  ../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp
-  ../../examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h
-  ../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp
-  ../../examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h
-  ../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp
-  ../../examples/SharedMemory/plugins/stablePDPlugin/KinTree.h
-  ../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp
-  ../../examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h
 	../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
 	../../examples/SharedMemory/GraphicsServerExample.cpp
 	../../examples/SharedMemory/GraphicsClientExample.cpp
@@ -146,7 +131,8 @@ ELSE(WIN32)
 	ENDIF(BUILD_CLSOCKET)
 		
 	IF(NOT APPLE)
-		LINK_LIBRARIES(  pthread ${DL} )
+		FIND_PACKAGE(Threads)
+		LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
 	ENDIF(NOT APPLE)
 ENDIF(WIN32)
 

Extras/CMakeLists.txt

@@ -1,5 +1,36 @@
-SUBDIRS( InverseDynamics BulletRoboticsGUI BulletRobotics obj2sdf Serialize ConvexDecomposition HACD GIMPACTUtils )
+OPTION(BUILD_INVERSE_DYNAMIC_EXTRA "Build InverseDynamic extra module, only applied when BUILD_EXTRAS is ON" ON)
+OPTION(BUILD_BULLET_ROBOTICS_GUI_EXTRA "Build BulletRoboticsGUI extra module, only applied when BUILD_EXTRAS is ON" ON)
+OPTION(BUILD_BULLET_ROBOTICS_EXTRA "Build BulletRobotics extra module, only applied when BUILD_EXTRAS is ON" ON)
+OPTION(BUILD_OBJ2SDF_EXTRA "Build obj2sdf extra module, only applied when BUILD_EXTRAS is ON" ON)
+OPTION(BUILD_SERIALIZE_EXTRA "Build Serialize extra module, only applied when BUILD_EXTRAS is ON" ON)
+OPTION(BUILD_CONVEX_DECOMPOSITION_EXTRA "Build ConvexDecomposition extra module, only applied when BUILD_EXTRAS is ON" ON)
+OPTION(BUILD_HACD_EXTRA "Build HACD extra module, only applied when BUILD_EXTRAS is ON" ON)
+OPTION(BUILD_GIMPACTUTILS_EXTRA "Build GIMPACTUtils extra module, only applied when BUILD_EXTRAS is ON" ON)
 
+IF(BUILD_INVERSE_DYNAMIC_EXTRA)
+  SUBDIRS( InverseDynamics )
+ENDIF()
+IF(BUILD_BULLET_ROBOTICS_GUI_EXTRA)
+  SUBDIRS( BulletRoboticsGUI )
+ENDIF()
+IF(BUILD_BULLET_ROBOTICS_EXTRA)
+  SUBDIRS( BulletRobotics )
+ENDIF()
+IF(BUILD_OBJ2SDF_EXTRA)
+  SUBDIRS( obj2sdf )
+ENDIF()
+IF(BUILD_SERIALIZE_EXTRA)
+  SUBDIRS( Serialize )
+ENDIF()
+IF(BUILD_CONVEX_DECOMPOSITION_EXTRA)
+  SUBDIRS( ConvexDecomposition )
+ENDIF()
+IF(BUILD_HACD_EXTRA)
+  SUBDIRS( HACD )
+ENDIF()
+IF(BUILD_GIMPACTUTILS_EXTRA)
+  SUBDIRS( GIMPACTUtils )
+ENDIF()
 
 
 #Maya Dynamica plugin is moved to http://dynamica.googlecode.com

Extras/ConvexDecomposition/LICENSE.txt

@@ -0,0 +1,19 @@
+Copyright (c) 2004 Open Dynamics Framework Group
+					www.physicstools.org
+		All rights reserved.
+		Redistribution and use in source and binary forms, with or without modification, are permitted provided
+		that the following conditions are met:
+		Redistributions of source code must retain the above copyright notice, this list of conditions
+		and the following disclaimer.
+		Redistributions in binary form must reproduce the above copyright notice,
+		this list of conditions and the following disclaimer in the documentation
+		and/or other materials provided with the distribution.
+		Neither the name of the Open Dynamics Framework Group nor the names of its contributors may
+		be used to endorse or promote products derived from this software without specific prior written permission.
+		THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES,
+		INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+		DISCLAIMED. IN NO EVENT SHALL THE INTEL OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+		EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+		LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
+		IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+		THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Extras/GIMPACTUtils/LICENSE.txt

@@ -0,0 +1,11 @@
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.

Extras/HACD/LICENSE.txt

@@ -0,0 +1,13 @@
+Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+ 
+ 
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+ 
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+ 
+ 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+ 
+ 3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission.
+ 
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Extras/InverseDynamics/LICENSE.txt

@@ -0,0 +1,12 @@
+Bullet Continuous Collision Detection and Physics Library
+http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.

Extras/Serialize/BulletFileLoader/bFile.cpp

@@ -75,6 +75,7 @@ bFile::bFile(const char *filename, const char headerString[7])
 		fseek(fp, 0L, SEEK_SET);
 
 		mFileBuffer = (char *)malloc(mFileLen + 1);
+                memset(mFileBuffer, 0, mFileLen+1);
 		size_t bytesRead;
 		bytesRead = fread(mFileBuffer, mFileLen, 1, fp);
 
@@ -643,8 +644,8 @@ char *bFile::readStruct(char *head, bChunkInd &dataChunk)
 			if ((strcmp(oldType, "btShortIntIndexData") == 0))
 			{
 				int allocLen = 2;
-				char *dataAlloc = new char[(dataChunk.nr * allocLen) + 1];
-				memset(dataAlloc, 0, (dataChunk.nr * allocLen) + 1);
+				char *dataAlloc = new char[(dataChunk.nr * allocLen) + sizeof(void*)];
+				memset(dataAlloc, 0, (dataChunk.nr * allocLen) + sizeof(void*));
 				short *dest = (short *)dataAlloc;
 				const short *src = (short *)head;
 				for (int i = 0; i < dataChunk.nr; i++)
@@ -682,8 +683,8 @@ char *bFile::readStruct(char *head, bChunkInd &dataChunk)
 				// numBlocks * length
 
 				int allocLen = (curLen);
-				char *dataAlloc = new char[(dataChunk.nr * allocLen) + 1];
-				memset(dataAlloc, 0, (dataChunk.nr * allocLen));
+				char *dataAlloc = new char[(dataChunk.nr * allocLen) + sizeof(void*)];
+				memset(dataAlloc, 0, (dataChunk.nr * allocLen) + sizeof(void*));
 
 				// track allocated
 				addDataBlock(dataAlloc);
@@ -719,8 +720,8 @@ char *bFile::readStruct(char *head, bChunkInd &dataChunk)
 #endif  //
 	}
 
-	char *dataAlloc = new char[(dataChunk.len) + 1];
-	memset(dataAlloc, 0, dataChunk.len + 1);
+	char *dataAlloc = new char[(dataChunk.len) + sizeof(void*)];
+	memset(dataAlloc, 0, dataChunk.len + sizeof(void*));
 
 	// track allocated
 	addDataBlock(dataAlloc);
@@ -736,7 +737,7 @@ void bFile::parseStruct(char *strcPtr, char *dtPtr, int old_dna, int new_dna, bo
 	if (new_dna == -1) return;
 
 	//disable this, because we need to fixup pointers/ListBase
-	if (0)  //mFileDNA->flagEqual(old_dna))
+	if (/* DISABLES CODE */ (0))  //mFileDNA->flagEqual(old_dna))
 	{
 		short *strc = mFileDNA->getStruct(old_dna);
 		int len = mFileDNA->getLength(strc[0]);

Extras/Serialize/BulletFileLoader/btBulletFile.cpp

@@ -333,6 +333,7 @@ void btBulletFile::parse(int verboseMode)
 		if (m_DnaCopy)
 			delete m_DnaCopy;
 		m_DnaCopy = (char*)btAlignedAlloc(sBulletDNAlen64, 16);
+                memset(m_DnaCopy, 0, sBulletDNAlen64);
 		memcpy(m_DnaCopy, sBulletDNAstr64, sBulletDNAlen64);
 		parseInternal(verboseMode, m_DnaCopy, sBulletDNAlen64);
 	}

Extras/Serialize/ReadBulletSample/main.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2011 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2011 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

Extras/Serialize/makesdna/DNA_rigidbody.h

@@ -1,3 +1,17 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
 
 #ifndef DNA_RIGIDBODY_H
 #define DNA_RIGIDBODY_H

Extras/VHACD/LICENSE.txt

@@ -0,0 +1,14 @@
+Copyright (c) 2011 Khaled Mamou (kmamou at gmail dot com)
+ All rights reserved.
+ 
+ 
+ Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+ 
+ 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+ 
+ 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
+ 
+ 3. The names of the contributors may not be used to endorse or promote products derived from this software without specific prior written permission.
+ 
+ THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ 

Extras/VHACD/inc/vhacdMutex.h

@@ -69,7 +69,7 @@
 #include <pthread.h>
 #endif
 
-#if defined(__APPLE__)
+#if defined(__APPLE__) || !defined(__GLIBC__)
 #define PTHREAD_MUTEX_RECURSIVE_NP PTHREAD_MUTEX_RECURSIVE
 #endif
 

Extras/obj2sdf/obj2sdf.cpp

@@ -4,6 +4,20 @@
 /// this will make it easier to load complex obj files into pybullet
 /// see for example export in data/kitchens/fathirmutfak.sdf
 
+///Bullet Continuous Collision Detection and Physics Library
+///Erwin Coumans (C) 2018
+///http://bulletphysics.org
+///
+///This software is provided 'as-is', without any express or implied warranty.
+///In no event will the authors be held liable for any damages arising from the use of this software.
+///Permission is granted to anyone to use this software for any purpose,
+///including commercial applications, and to alter it and redistribute it freely,
+///subject to the following restrictions:
+///
+///1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+///2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+///3. This notice may not be removed or altered from any source distribution.
+	
 #include <string.h>
 #include <stdio.h>
 #include <assert.h>
@@ -257,7 +271,7 @@ int main(int argc, char* argv[])
 			fprintf(sdfFile,
 					"\t\t<model name='%s'>\n"
 					"\t\t\t<static>1</static>\n"
-					"\t\t\t<pose frame=''>0 0 0 0 0 0</pose>\n"
+					"\t\t\t<pose >0 0 0 0 0 0</pose>\n"
 					"\t\t\t<link name='link_d%d'>\n"
 					"\t\t\t<inertial>\n"
 					"\t\t\t<mass>0</mass>\n"
@@ -397,7 +411,7 @@ int main(int argc, char* argv[])
 			fprintf(sdfFile,
 					"\t\t<model name='%s'>\n"
 					"\t\t\t<static>1</static>\n"
-					"\t\t\t<pose frame=''>0 0 0 0 0 0</pose>\n"
+					"\t\t\t<pose>0 0 0 0 0 0</pose>\n"
 					"\t\t\t<link name='link_d%d'>\n"
 					"\t\t\t<inertial>\n"
 					"\t\t\t<mass>0</mass>\n"

README.md

@@ -7,8 +7,11 @@ This is the official C++ source code repository of the Bullet Physics SDK: real-
 
 ![PyBullet](https://pybullet.org/wordpress/wp-content/uploads/2019/03/cropped-pybullet.png)
 
+## Issues ##
+The Issue tracker was flooded with support questions and is closed until it is cleaned up. Use the [PyBullet forums](http://pybullet.org) to discuss with others.
+
 ## PyBullet ##
-New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR. Use pip install pybullet and checkout the [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).
+It is highly recommended to use PyBullet Python bindings for improved support for robotics, reinforcement learning and VR. Use pip install pybullet and checkout the [PyBullet Quickstart Guide](https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.2ye70wns7io3).
 
 Installation is simple:
 ```
@@ -21,11 +24,11 @@ python3 -m pybullet_envs.deep_mimic.testrl --arg_file run_humanoid3d_backflip_ar
 If you use PyBullet in your research, please cite it like this:
 
 ```
-@MISC{coumans2019,
+@MISC{coumans2021,
 author =   {Erwin Coumans and Yunfei Bai},
 title =    {PyBullet, a Python module for physics simulation for games, robotics and machine learning},
 howpublished = {\url{http://pybullet.org}},
-year = {2016--2019}
+year = {2016--2021}
 }
 ```
 
@@ -63,7 +66,7 @@ You can download and install Bullet using the [vcpkg](https://github.com/Microso
     cd vcpkg
     ./bootstrap-vcpkg.sh
     ./vcpkg integrate install
-    vcpkg install bullet3
+    ./vcpkg install bullet3
 
 The Bullet port in vcpkg is kept up to date by Microsoft team members and community contributors. If the version is out of date, please [create an issue or pull request](https://github.com/Microsoft/vcpkg) on the vcpkg repository.
 
@@ -91,9 +94,9 @@ p.connect(p.SHARED_MEMORY) #or (p.TCP, "localhost", 6667) or (p.UDP, "192.168.86
 Make sure cmake is installed (sudo apt-get install cmake, brew install cmake, or https://cmake.org)
 
 In a terminal type:
-
-	./build_cmake_pybullet_double.sh
-
+```
+./build_cmake_pybullet_double.sh
+```
 This script will invoke cmake and build in the build_cmake directory. You can find pybullet in Bullet/examples/pybullet.
 The BulletExampleBrowser binary will be in Bullet/examples/ExampleBrowser.
 
@@ -101,15 +104,15 @@ You can also build Bullet using premake. There are premake executables in the bu
 Depending on your system (Linux 32bit, 64bit or Mac OSX) use one of the following lines
 Using premake:
 ```
-	cd build3
-	./premake4_linux --double gmake
-	./premake4_linux64 --double gmake
-	./premake4_osx --double --enable_pybullet gmake
+cd build3
+./premake4_linux --double gmake
+./premake4_linux64 --double gmake
+./premake4_osx --double --enable_pybullet gmake
 ```
 Then
 ```
-	cd gmake
-	make
+cd gmake
+make
 ```
 
 Note that on Linux, you need to use cmake to build pybullet, since the compiler has issues of mixing shared and static libraries.
@@ -117,9 +120,9 @@ Note that on Linux, you need to use cmake to build pybullet, since the compiler
 **Mac OSX Xcode**
 	
 Click on build3/xcode4.command or in a terminal window execute
-	
-	./premake_osx xcode4
-
+```	
+./premake_osx xcode4
+```
 ## Usage
 
 The App_ExampleBrowser executables will be located in the bin folder.

VERSION

@@ -1 +1 @@
-3.05
+3.20

build_visual_studio_vr_pybullet_double_cmake.bat

@@ -1,4 +1,4 @@
 mkdir build_cmake
 cd build_cmake
-cmake -DBUILD_PYBULLET=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python-3.5.3.amd64\include -DPYTHON_LIBRARY=c:\python-3.5.3.amd64\libs\python35.lib -DPYTHON_DEBUG_LIBRARY=c:\python-3.5.3.amd64\libs\python35_d.lib -G "Visual Studio 16 2019" ..
+cmake -DBUILD_PYBULLET=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python38\include -DPYTHON_LIBRARY=c:\python38\libs\python38.lib -DPYTHON_DEBUG_LIBRARY=c:\python38\libs\python38.lib -G "Visual Studio 16 2019" ..
 start .

data/cube_small.sdf

@@ -1,7 +1,7 @@
 <sdf version='1.6'>
   <world name='default'>
     <model name='unit_box_0'>
-      <pose frame=''>0 0 0.107 0 0 0</pose>
+      <pose>0 0 0.107 0 0 0</pose>
       <link name='unit_box_0::link'>
         <inertial>
           <mass>0.1</mass>

data/gripper/wsg50_one_motor_gripper.sdf

@@ -2,7 +2,7 @@
 <sdf version='1.6'>
   <world name='default'>
   <model name='wsg50_with_gripper'>
-    <pose frame=''>0 0 0.26 3.14 0 0</pose>
+    <pose>0 0 0.26 3.14 0 0</pose>
 	
     <link name='world'>
 	</link>
@@ -28,9 +28,9 @@
     </joint>
 
     <link name='base_link'>
-      <pose frame=''>0 0 0 0 0 0</pose>
+      <pose>0 0 0 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>1.2</mass>
         <inertia>
           <ixx>1</ixx>
@@ -43,7 +43,7 @@
       </inertial>
 	  
       <visual name='base_link_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -58,9 +58,9 @@
     </link>
 	
     <link name='motor'>
-      <pose frame=''>0 0 0.03 0 0 0</pose>
+      <pose>0 0 0.03 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -72,7 +72,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0 0 0.01 0 0 0</pose>
+        <pose>0 0 0.01 0 0 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.02 </size>
@@ -102,9 +102,9 @@
     </joint>
 	
     <link name='left_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -116,7 +116,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
+        <pose>-0.03 0 0.01 0 -1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -147,9 +147,9 @@
     </joint>
 	
     <link name='right_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -161,7 +161,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0.03 0 0.01 0 1.2 0</pose>
+        <pose>0.03 0 0.01 0 1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -192,9 +192,9 @@
     </joint>
 
     <link name='gripper_left'>
-      <pose frame=''>-0.055 0 0.06 0 -0 0</pose>
+      <pose>-0.055 0 0.06 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -207,7 +207,7 @@
       </inertial>
 	  
       <visual name='gripper_left_visual'>
-        <pose frame=''>0 0 -0.06 0 0 0</pose>
+        <pose>0 0 -0.06 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -216,7 +216,7 @@
         </geometry>
       </visual>
       <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 0</pose>
+        <pose>0 0 -0.037 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -249,9 +249,9 @@
     </joint>
 	
     <link name='gripper_right'>
-      <pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
+      <pose>0.055 0 0.06 0 0 3.14159</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -264,7 +264,7 @@
       </inertial>
 	  
       <visual name='gripper_right_visual'>
-        <pose frame=''>0 0 -0.06 0 0 0</pose>
+        <pose>0 0 -0.06 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -273,7 +273,7 @@
         </geometry>
       </visual>
       <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 0</pose>
+        <pose>0 0 -0.037 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -305,7 +305,7 @@
     </joint>
     
     <link name='finger_right'>
-      <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
+      <pose>0.062 0 0.145 0 0 1.5708</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -319,7 +319,7 @@
       </inertial>
 	  
         <collision name='finger_right_collision'>
-  	<pose frame=''>0 0 0.042 0 0 0 </pose>
+  	<pose>0 0 0.042 0 0 0 </pose>
   	<geometry>        
          <box>
                 <size>0.02 0.02 0.15 </size>
@@ -329,7 +329,7 @@
         </collision>
 	  
       <visual name='finger_right_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -345,7 +345,7 @@
     </joint>
     
     <link name='finger_left'>
-      <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
+      <pose>-0.062 0 0.145 0 0 4.71239</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -359,7 +359,7 @@
       </inertial>
 	  
       <collision name='finger_left_collision'>
-        <pose frame=''>0 0 0.042 0 0 0 </pose>
+        <pose>0 0 0.042 0 0 0 </pose>
         <geometry>
                <box>
               <size>0.02 0.02 0.15 </size>
@@ -369,7 +369,7 @@
       </collision>
 	  
       <visual name='finger_left_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>

data/gripper/wsg50_one_motor_gripper_free_base.sdf

@@ -3,11 +3,11 @@
   <world name='default'>
   <model name='wsg50_with_gripper'>
     
-    <pose frame=''>0 -2.3 2.1 0 0 0</pose>
+    <pose>0 -2.3 2.1 0 0 0</pose>
     <link name='world'>
-	<pose frame=''>0 0 0 0 0 0</pose>
+	<pose>0 0 0 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>1</ixx>
@@ -26,9 +26,9 @@
     </joint>
 
     <link name='base_link'>
-      <pose frame=''>0 0 0 0 0 0</pose>
+      <pose>0 0 0 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>1.2</mass>
         <inertia>
           <ixx>1</ixx>
@@ -41,7 +41,7 @@
       </inertial>
 	  
       <visual name='base_link_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -56,9 +56,9 @@
     </link>
 	
     <link name='motor'>
-      <pose frame=''>0 0 0.03 0 0 0</pose>
+      <pose>0 0 0.03 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -70,7 +70,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0 0 0.01 0 0 0</pose>
+        <pose>0 0 0.01 0 0 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.02 </size>
@@ -100,9 +100,9 @@
     </joint>
 	
     <link name='left_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -114,7 +114,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
+        <pose>-0.03 0 0.01 0 -1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -145,9 +145,9 @@
     </joint>
 	
     <link name='right_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -159,7 +159,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0.03 0 0.01 0 1.2 0</pose>
+        <pose>0.03 0 0.01 0 1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -190,9 +190,9 @@
     </joint>
 
     <link name='gripper_left'>
-      <pose frame=''>-0.055 0 0.06 0 -0 0</pose>
+      <pose>-0.055 0 0.06 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -205,7 +205,7 @@
       </inertial>
 	  
       <visual name='gripper_left_visual'>
-        <pose frame=''>0 0 -0.06 0 0 0</pose>
+        <pose>0 0 -0.06 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -214,7 +214,7 @@
         </geometry>
       </visual>
       <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 0</pose>
+        <pose>0 0 -0.037 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -247,9 +247,9 @@
     </joint>
 	
     <link name='gripper_right'>
-      <pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
+      <pose>0.055 0 0.06 0 0 3.14159</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -262,7 +262,7 @@
       </inertial>
 	  
       <visual name='gripper_right_visual'>
-        <pose frame=''>0 0 -0.06 0 0 0</pose>
+        <pose>0 0 -0.06 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -271,7 +271,7 @@
         </geometry>
       </visual>
       <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 0</pose>
+        <pose>0 0 -0.037 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -307,7 +307,7 @@
         <lateral_friction value="1.0"/>
         <spinning_friction value="1.5"/>
       </contact>
-      <pose frame=''>0.042 0 0.145 0 0 1.5708</pose>
+      <pose>0.042 0 0.145 0 0 1.5708</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -321,7 +321,7 @@
       </inertial>
 	  
         <collision name='finger_right_collision'>
-  	<pose frame=''>0 0 0.042 0 0 0 </pose>
+  	<pose>0 0 0.042 0 0 0 </pose>
   	<geometry>        
          <box>
                 <size>0.02 0.02 0.15 </size>
@@ -331,7 +331,7 @@
         </collision>
 	  
       <visual name='finger_right_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -351,7 +351,7 @@
         <lateral_friction value="1.0"/>
         <spinning_friction value="1.5"/>
       </contact>
-      <pose frame=''>-0.042 0 0.145 0 0 4.71239</pose>
+      <pose>-0.042 0 0.145 0 0 4.71239</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -365,7 +365,7 @@
       </inertial>
 	  
       <collision name='finger_left_collision'>
-        <pose frame=''>0 0 0.042 0 0 0 </pose>
+        <pose>0 0 0.042 0 0 0 </pose>
         <geometry>
                <box>
               <size>0.02 0.02 0.15 </size>
@@ -375,7 +375,7 @@
       </collision>
 	  
       <visual name='finger_left_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>

data/gripper/wsg50_one_motor_gripper_new.sdf

@@ -2,7 +2,7 @@
 <sdf version='1.6'>
   <world name='default'>
   <model name='wsg50_with_gripper'>
-    <pose frame=''>0 0 0.7 3.14 0 0</pose>
+    <pose>0 0 0.7 3.14 0 0</pose>
 	
     <link name='world'>
 	</link>
@@ -28,9 +28,9 @@
     </joint>
 
     <link name='base_link'>
-      <pose frame=''>0 0 0 0 0 0</pose>
+      <pose>0 0 0 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>1.2</mass>
         <inertia>
           <ixx>1</ixx>
@@ -43,7 +43,7 @@
       </inertial>
 	  
       <visual name='base_link_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -58,9 +58,9 @@
     </link>
 	
     <link name='motor'>
-      <pose frame=''>0 0 0.03 0 0 0</pose>
+      <pose>0 0 0.03 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -72,7 +72,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0 0 0.01 0 0 0</pose>
+        <pose>0 0 0.01 0 0 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.02 </size>
@@ -102,9 +102,9 @@
     </joint>
 	
     <link name='left_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -116,7 +116,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
+        <pose>-0.03 0 0.01 0 -1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -147,9 +147,9 @@
     </joint>
 	
     <link name='right_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -161,7 +161,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0.03 0 0.01 0 1.2 0</pose>
+        <pose>0.03 0 0.01 0 1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -192,9 +192,9 @@
     </joint>
 
     <link name='gripper_left'>
-      <pose frame=''>-0.055 0 0.06 0 -0 0</pose>
+      <pose>-0.055 0 0.06 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -207,7 +207,7 @@
       </inertial>
 	  
       <visual name='gripper_left_visual'>
-        <pose frame=''>0 0 -0.06 0 0 0</pose>
+        <pose>0 0 -0.06 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -216,7 +216,7 @@
         </geometry>
       </visual>
       <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 0</pose>
+        <pose>0 0 -0.037 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -248,9 +248,9 @@
     </joint>
 	
     <link name='gripper_right'>
-      <pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
+      <pose>0.055 0 0.06 0 0 3.14159</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -263,7 +263,7 @@
       </inertial>
 	  
       <visual name='gripper_right_visual'>
-        <pose frame=''>0 0 -0.06 0 0 0</pose>
+        <pose>0 0 -0.06 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -272,7 +272,7 @@
         </geometry>
       </visual>
       <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 0</pose>
+        <pose>0 0 -0.037 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -307,7 +307,7 @@
         <spinning_friction>.3</spinning_friction>
         <rolling_friction>0.04</rolling_friction>
       </contact>
-      <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
+      <pose>0.062 0 0.145 0 0 1.5708</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -321,7 +321,7 @@
       </inertial>
 	  
         <collision name='finger_right_collision'>
-  	<pose frame=''>0 0 0.042 0 0 0 </pose>
+  	<pose>0 0 0.042 0 0 0 </pose>
   	<geometry>        
          <box>
                 <size>0.02 0.02 0.15 </size>
@@ -331,7 +331,7 @@
         </collision>
 	  
       <visual name='finger_right_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -351,7 +351,7 @@
         <spinning_friction>.3</spinning_friction>
         <rolling_friction>0.04</rolling_friction>
       </contact>
-      <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
+      <pose>-0.062 0 0.145 0 0 4.71239</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -365,7 +365,7 @@
       </inertial>
 	  
       <collision name='finger_left_collision'>
-        <pose frame=''>0 0 0.042 0 0 0 </pose>
+        <pose>0 0 0.042 0 0 0 </pose>
         <geometry>
                <box>
               <size>0.02 0.02 0.15 </size>
@@ -375,7 +375,7 @@
       </collision>
 	  
       <visual name='finger_left_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>

data/gripper/wsg50_one_motor_gripper_new_free_base.sdf

@@ -2,11 +2,11 @@
 <sdf version='1.6'>
   <world name='default'>
   <model name='wsg50_with_gripper'>
-    <pose frame=''>1.4 -0.2 2.1 0 0 0</pose>
+    <pose>1.4 -0.2 2.1 0 0 0</pose>
     <link name='world'>
-	<pose frame=''>0 0 0 0 0 0</pose>
+	<pose>0 0 0 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>1</ixx>
@@ -25,9 +25,9 @@
     </joint>
 
     <link name='base_link'>
-      <pose frame=''>0 0 0 0 0 0</pose>
+      <pose>0 0 0 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>1.2</mass>
         <inertia>
           <ixx>1</ixx>
@@ -40,7 +40,7 @@
       </inertial>
 	  
       <visual name='base_link_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -55,9 +55,9 @@
     </link>
 	
     <link name='motor'>
-      <pose frame=''>0 0 0.03 0 0 0</pose>
+      <pose>0 0 0.03 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -69,7 +69,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0 0 0.01 0 0 0</pose>
+        <pose>0 0 0.01 0 0 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.02 </size>
@@ -99,9 +99,9 @@
     </joint>
 	
     <link name='left_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -113,7 +113,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
+        <pose>-0.03 0 0.01 0 -1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -144,9 +144,9 @@
     </joint>
 	
     <link name='right_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -158,7 +158,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0.03 0 0.01 0 1.2 0</pose>
+        <pose>0.03 0 0.01 0 1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -189,9 +189,9 @@
     </joint>
 
     <link name='gripper_left'>
-      <pose frame=''>-0.055 0 0.06 0 -0 0</pose>
+      <pose>-0.055 0 0.06 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -204,7 +204,7 @@
       </inertial>
 	  
       <visual name='gripper_left_visual'>
-        <pose frame=''>0 0 -0.06 0 0 0</pose>
+        <pose>0 0 -0.06 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -213,7 +213,7 @@
         </geometry>
       </visual>
       <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 0</pose>
+        <pose>0 0 -0.037 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -245,9 +245,9 @@
     </joint>
 	
     <link name='gripper_right'>
-      <pose frame=''>0.055 0 0.06 0 0 3.14159</pose>
+      <pose>0.055 0 0.06 0 0 3.14159</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -260,7 +260,7 @@
       </inertial>
 	  
       <visual name='gripper_right_visual'>
-        <pose frame=''>0 0 -0.06 0 0 0</pose>
+        <pose>0 0 -0.06 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -269,7 +269,7 @@
         </geometry>
       </visual>
       <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 0</pose>
+        <pose>0 0 -0.037 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -304,7 +304,7 @@
         <lateral_friction>1.0</lateral_friction>
         <spinning_friction>1.5</spinning_friction>
       </contact>
-      <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
+      <pose>0.062 0 0.145 0 0 1.5708</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -318,7 +318,7 @@
       </inertial>
 	  
         <collision name='finger_right_collision'>
-  	<pose frame=''>0 0 0.042 0 0 0 </pose>
+  	<pose>0 0 0.042 0 0 0 </pose>
   	<geometry>        
          <box>
                 <size>0.02 0.02 0.15 </size>
@@ -328,7 +328,7 @@
         </collision>
 	  
       <visual name='finger_right_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -348,7 +348,7 @@
         <lateral_friction>1.0</lateral_friction>
         <spinning_friction>1.5</spinning_friction>
       </contact>
-      <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
+      <pose>-0.062 0 0.145 0 0 4.71239</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -362,7 +362,7 @@
       </inertial>
 	  
       <collision name='finger_left_collision'>
-        <pose frame=''>0 0 0.042 0 0 0 </pose>
+        <pose>0 0 0.042 0 0 0 </pose>
         <geometry>
                <box>
               <size>0.02 0.02 0.15 </size>
@@ -372,7 +372,7 @@
       </collision>
 	  
       <visual name='finger_left_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>

data/gripper/wsg50_one_motor_gripper_no_finger.sdf

@@ -2,7 +2,7 @@
 <sdf version='1.6'>
   <world name='default'>
   <model name='wsg50_with_gripper'>
-    <pose frame=''>0 0 0.4 3.14 0 0</pose>
+    <pose>0 0 0.4 3.14 0 0</pose>
 	
     <link name='world'>
 	</link>
@@ -28,9 +28,9 @@
     </joint>
 
     <link name='base_link'>
-      <pose frame=''>0 0 0 0 0 0</pose>
+      <pose>0 0 0 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>1.2</mass>
         <inertia>
           <ixx>1</ixx>
@@ -43,7 +43,7 @@
       </inertial>
 	  
       <visual name='base_link_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -58,9 +58,9 @@
     </link>
 	
     <link name='motor'>
-      <pose frame=''>0 0 0.03 0 0 0</pose>
+      <pose>0 0 0.03 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -72,7 +72,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0 0 0.01 0 0 0</pose>
+        <pose>0 0 0.01 0 0 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.02 </size>
@@ -102,9 +102,9 @@
     </joint>
 	
     <link name='left_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -116,7 +116,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>-0.03 0 0.01 0 -1.2 0</pose>
+        <pose>-0.03 0 0.01 0 -1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -147,9 +147,9 @@
     </joint>
 	
     <link name='right_hinge'>
-      <pose frame=''>0 0 0.04 0 0 0</pose>
+      <pose>0 0 0.04 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.035 0 0 0</pose>
+        <pose>0 0 0.035 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -161,7 +161,7 @@
         </inertia>
       </inertial>
       <visual name='motor_visual'>
-        <pose frame=''>0.03 0 0.01 0 1.2 0</pose>
+        <pose>0.03 0 0.01 0 1.2 0</pose>
         <geometry>
 		  <box>
             <size>0.02 0.02 0.07 </size>
@@ -192,9 +192,9 @@
     </joint>
 
     <link name='gripper_left'>
-      <pose frame=''>-0.055 0 0.06 0 -0 0</pose>
+      <pose>-0.055 0 0.06 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -207,7 +207,7 @@
       </inertial>
 	  
       <visual name='gripper_left_visual'>
-        <pose frame=''>0 0 -0.06 0 0 0</pose>
+        <pose>0 0 -0.06 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -216,7 +216,7 @@
         </geometry>
       </visual>
       <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 0</pose>
+        <pose>0 0 -0.037 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -248,9 +248,9 @@
     </joint>
 	
     <link name='gripper_right'>
-      <pose frame=''>0.055 0 0.06 0 0 0</pose>
+      <pose>0.055 0 0.06 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -263,7 +263,7 @@
       </inertial>
 	  
       <visual name='gripper_right_visual'>
-        <pose frame=''>0 0 -0.06 0 0 3.14159</pose>
+        <pose>0 0 -0.06 0 0 3.14159</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -272,7 +272,7 @@
         </geometry>
       </visual>
       <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
-        <pose frame=''>0 0 -0.037 0 0 3.14159</pose>
+        <pose>0 0 -0.037 0 0 3.14159</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>

data/gripper/wsg50_with_r2d2_gripper.sdf

@@ -2,7 +2,7 @@
 <sdf version='1.6'>
   <world name='default'>
   <model name='wsg50_with_gripper'>
-    <pose frame=''>0 0 0.27 3.14 0 0</pose>
+    <pose>0 0 0.27 3.14 0 0</pose>
 	
     <link name='world'>
 	</link>
@@ -28,9 +28,9 @@
     </joint>
 
     <link name='base_link'>
-      <pose frame=''>0 0 0 0 -0 0</pose>
+      <pose>0 0 0 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <mass>1.2</mass>
         <inertia>
           <ixx>1</ixx>
@@ -42,7 +42,7 @@
         </inertia>
       </inertial>
       <collision name='base_link_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
 		<box>
               <size>0.2 0.05 0.05 </size>
@@ -50,7 +50,7 @@
         </geometry>
       </collision>
       <visual name='base_link_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -67,9 +67,9 @@
     </link>
 
     <link name='gripper_left'>
-      <pose frame=''>-0.055 0 0 0 -0 0</pose>
+      <pose>-0.055 0 0 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -82,7 +82,7 @@
       </inertial>
 	  
       <collision name='gripper_left_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -91,7 +91,7 @@
         </geometry>
       </collision>
       <collision name='gripper_left_fixed_joint_lump__finger_left_collision_1'>
-        <pose frame=''>0 0 0.023 0 -0 0</pose>
+        <pose>0 0 0.023 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -101,7 +101,7 @@
       </collision>
 	  
       <visual name='gripper_left_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -110,7 +110,7 @@
         </geometry>
       </visual>
       <visual name='gripper_left_fixed_joint_lump__finger_left_visual_1'>
-        <pose frame=''>0 0 0.023 0 -0 0</pose>
+        <pose>0 0 0.023 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -141,9 +141,9 @@
     </joint>
 	
     <link name='gripper_right'>
-      <pose frame=''>0.055 0 0 0 -0 3.14159</pose>
+      <pose>0.055 0 0 0 -0 3.14159</pose>
       <inertial>
-        <pose frame=''>0 0 0.0115 0 -0 0</pose>
+        <pose>0 0 0.0115 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -156,7 +156,7 @@
       </inertial>
 	  
       <collision name='gripper_right_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -165,7 +165,7 @@
         </geometry>
       </collision>
       <collision name='gripper_right_fixed_joint_lump__finger_right_collision_1'>
-        <pose frame=''>0 0 0.023 0 -0 0</pose>
+        <pose>0 0 0.023 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -175,7 +175,7 @@
       </collision>
 	  
       <visual name='gripper_right_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -184,7 +184,7 @@
         </geometry>
       </visual>
       <visual name='gripper_right_fixed_joint_lump__finger_right_visual_1'>
-        <pose frame=''>0 0 0.023 0 -0 0</pose>
+        <pose>0 0 0.023 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>0.001 0.001 0.001</scale>
@@ -219,7 +219,7 @@
         <lateral_friction>1.0</lateral_friction>
         <spinning_friction>1.5</spinning_friction>
       </contact>
-      <pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
+      <pose>0.062 0 0.145 0 0 1.5708</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -233,7 +233,7 @@
       </inertial>
 	  
       <collision name='finger_right_collision'>
-	<pose frame=''>0 0 0.042 0 0 0 </pose>
+	<pose>0 0 0.042 0 0 0 </pose>
 	<geometry>        
        <box>
               <size>0.02 0.02 0.15 </size>
@@ -243,7 +243,7 @@
       </collision>
 	  
       <visual name='finger_right_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -263,7 +263,7 @@
         <lateral_friction>1.0</lateral_friction>
         <spinning_friction>1.5</spinning_friction>
       </contact>
-      <pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
+      <pose>-0.062 0 0.145 0 0 4.71239</pose>
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -277,7 +277,7 @@
       </inertial>
 	  
       <collision name='finger_left_collision'>
-        <pose frame=''>0 0 0.042 0 0 0 </pose>
+        <pose>0 0 0.042 0 0 0 </pose>
         <geometry>
                <box>
               <size>0.02 0.02 0.15 </size>
@@ -287,7 +287,7 @@
       </collision>
 	  
       <visual name='finger_left_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>

data/kuka_iiwa/kuka_with_gripper.sdf

@@ -11,9 +11,9 @@
   <world name='default'>
   <model name='lbr_iiwa_with_wsg50'>
     <link name='lbr_iiwa_link_0'>
-      <pose frame=''>0 0 0 0 -0 0</pose>
+      <pose>0 0 0 0 -0 0</pose>
       <inertial>
-        <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+        <pose>-0.1 0 0.07 0 -0 0</pose>
         <mass>0</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -25,7 +25,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_0_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -34,7 +34,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_0_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -50,9 +50,9 @@
       </visual>
     </link>
     <link name='lbr_iiwa_link_1'>
-      <pose frame=''>0 0 0.1575 0 -0 0</pose>
+      <pose>0 0 0.1575 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+        <pose>0 -0.03 0.12 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -64,7 +64,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_1_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -73,7 +73,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_1_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -108,9 +108,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_2'>
-      <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+      <pose>0 0 0.36 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+        <pose>0.0003 0.059 0.042 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -122,7 +122,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_2_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -131,7 +131,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_2_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -166,9 +166,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_3'>
-      <pose frame=''>0 -0 0.5645 0 0 0</pose>
+      <pose>0 -0 0.5645 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+        <pose>0 0.03 0.13 0 -0 0</pose>
         <mass>3</mass>
         <inertia>
           <ixx>0.08</ixx>
@@ -180,7 +180,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_3_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -189,7 +189,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_3_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -224,9 +224,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_4'>
-      <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+      <pose>0 -0 0.78 1.5708 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+        <pose>0 0.067 0.034 0 -0 0</pose>
         <mass>2.7</mass>
         <inertia>
           <ixx>0.03</ixx>
@@ -238,7 +238,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_4_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -247,7 +247,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_4_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -282,9 +282,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_5'>
-      <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+      <pose>0 -0 0.9645 0 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+        <pose>0.0001 0.021 0.076 0 -0 0</pose>
         <mass>1.7</mass>
         <inertia>
           <ixx>0.02</ixx>
@@ -296,7 +296,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_5_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -305,7 +305,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_5_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -340,9 +340,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_6'>
-      <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+      <pose>0 0 1.18 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+        <pose>0 0.0006 0.0004 0 -0 0</pose>
         <mass>1.8</mass>
         <inertia>
           <ixx>0.005</ixx>
@@ -354,7 +354,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_6_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -363,7 +363,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_6_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -398,9 +398,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_7'>
-      <pose frame=''>0 0 1.261 0 0 0</pose>
+      <pose>0 0 1.261 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.02 0 -0 0</pose>
+        <pose>0 0 0.02 0 -0 0</pose>
         <mass>0.3</mass>
         <inertia>
           <ixx>0.001</ixx>
@@ -412,7 +412,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_7_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -421,7 +421,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_7_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -462,9 +462,9 @@
       <child>base_link</child>
     </joint>
     <link name='base_link'>
-      <pose frame=''>0 0 1.305 0 -0 0</pose>
+      <pose>0 0 1.305 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <mass>1.2</mass>
         <inertia>
           <ixx>1</ixx>
@@ -476,7 +476,7 @@
         </inertia>
       </inertial>
       <visual name='base_link_visual'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
 		  <box>
 		     <size>0.05 0.05 0.1 </size>
@@ -511,9 +511,9 @@
       </axis>
     </joint>
     <link name='left_finger'>
-      <pose frame=''>0 0.024 1.35 0 -0.05 0</pose>
+      <pose>0 0.024 1.35 0 -0.05 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -525,7 +525,7 @@
         </inertia>
       </inertial>
       <visual name='left_finger_visual'>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <geometry>
           <box>
             <size>0.01 0.01 0.08</size>
@@ -544,9 +544,9 @@
       <child>left_finger_base</child>
     </joint>
     <link name='left_finger_base'>
-      <pose frame=''>-0.005 0.024 1.43 0 -0.3 0</pose>
+      <pose>-0.005 0.024 1.43 0 -0.3 0</pose>
       <inertial>
-        <pose frame=''>-0.003 0 0.04 0 0 0 </pose>
+        <pose>-0.003 0 0.04 0 0 0 </pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -558,7 +558,7 @@
         </inertia>
       </inertial>
       <visual name='left_finger_base_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -573,7 +573,7 @@
         </material>
       </visual>
       <collision name='left_finger_base_collision'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -606,9 +606,9 @@
         <lateral_friction>0.8</lateral_friction>
         <spinning_friction>1.0</spinning_friction>
       </contact>
-      <pose frame=''>-0.02 0.024 1.49 0 0.2 0</pose>
+      <pose>-0.02 0.024 1.49 0 0.2 0</pose>
       <inertial>
-        <pose frame=''>-0.005 0 0.026 0 0 0 </pose>
+        <pose>-0.005 0 0.026 0 0 0 </pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -620,7 +620,7 @@
         </inertia>
       </inertial>
       <visual name='left_finger_tip_visual'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -635,7 +635,7 @@
         </material>
       </visual>
       <collision name='left_finger_tip_collision'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -664,9 +664,9 @@
       </axis>
     </joint>
     <link name='right_finger'>
-      <pose frame=''>0 0.024 1.35 0 0.05 0</pose>
+      <pose>0 0.024 1.35 0 0.05 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <mass>0.1</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -678,7 +678,7 @@
         </inertia>
       </inertial>
       <visual name='right_finger_visual'>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <geometry>
           <box>
             <size>0.01 0.01 0.08</size>
@@ -697,9 +697,9 @@
       <child>right_finger_base</child>
     </joint>
     <link name='right_finger_base'>
-      <pose frame=''>0.005 0.024 1.43 0 0.3 0</pose>
+      <pose>0.005 0.024 1.43 0 0.3 0</pose>
       <inertial>
-        <pose frame=''>0.003 0 0.04 0 0 0 </pose>
+        <pose>0.003 0 0.04 0 0 0 </pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -711,7 +711,7 @@
         </inertia>
       </inertial>
       <visual name='right_finger_base_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -726,7 +726,7 @@
         </material>
       </visual>
       <collision name='right_finger_base_collision'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -759,9 +759,9 @@
         <lateral_friction>0.8</lateral_friction>
         <spinning_friction>1.0</spinning_friction>
       </contact>
-      <pose frame=''>0.02 0.024 1.49 0 -0.2 0</pose>
+      <pose>0.02 0.024 1.49 0 -0.2 0</pose>
       <inertial>
-        <pose frame=''>0.005 0 0.026 0 0 0 </pose>
+        <pose>0.005 0 0.026 0 0 0 </pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -773,7 +773,7 @@
         </inertia>
       </inertial>
       <visual name='right_finger_visual'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -788,7 +788,7 @@
         </material>
       </visual>
       <collision name='right_finger_tip_collision'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>

data/kuka_iiwa/kuka_with_gripper2.sdf

@@ -11,9 +11,9 @@
   <world name='default'>
   <model name='lbr_iiwa_with_wsg50'>
     <link name='lbr_iiwa_link_0'>
-      <pose frame=''>0 0 0 0 -0 0</pose>
+      <pose>0 0 0 0 -0 0</pose>
       <inertial>
-        <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+        <pose>-0.1 0 0.07 0 -0 0</pose>
         <mass>0</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -25,7 +25,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_0_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -34,7 +34,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_0_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -50,9 +50,9 @@
       </visual>
     </link>
     <link name='lbr_iiwa_link_1'>
-      <pose frame=''>0 0 0.1575 0 -0 0</pose>
+      <pose>0 0 0.1575 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+        <pose>0 -0.03 0.12 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -64,7 +64,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_1_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -73,7 +73,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_1_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -108,9 +108,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_2'>
-      <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+      <pose>0 0 0.36 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+        <pose>0.0003 0.059 0.042 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -122,7 +122,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_2_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -131,7 +131,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_2_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -166,9 +166,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_3'>
-      <pose frame=''>0 -0 0.5645 0 0 0</pose>
+      <pose>0 -0 0.5645 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+        <pose>0 0.03 0.13 0 -0 0</pose>
         <mass>3</mass>
         <inertia>
           <ixx>0.08</ixx>
@@ -180,7 +180,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_3_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -189,7 +189,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_3_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -224,9 +224,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_4'>
-      <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+      <pose>0 -0 0.78 1.5708 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+        <pose>0 0.067 0.034 0 -0 0</pose>
         <mass>2.7</mass>
         <inertia>
           <ixx>0.03</ixx>
@@ -238,7 +238,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_4_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -247,7 +247,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_4_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -282,9 +282,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_5'>
-      <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+      <pose>0 -0 0.9645 0 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+        <pose>0.0001 0.021 0.076 0 -0 0</pose>
         <mass>1.7</mass>
         <inertia>
           <ixx>0.02</ixx>
@@ -296,7 +296,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_5_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -305,7 +305,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_5_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -340,9 +340,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_6'>
-      <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+      <pose>0 0 1.18 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+        <pose>0 0.0006 0.0004 0 -0 0</pose>
         <mass>1.8</mass>
         <inertia>
           <ixx>0.005</ixx>
@@ -354,7 +354,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_6_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -363,7 +363,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_6_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -398,9 +398,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_7'>
-      <pose frame=''>0 0 1.261 0 0 0</pose>
+      <pose>0 0 1.261 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.02 0 -0 0</pose>
+        <pose>0 0 0.02 0 -0 0</pose>
         <mass>1.3</mass>
         <inertia>
           <ixx>0.001</ixx>
@@ -412,7 +412,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_7_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -421,7 +421,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_7_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -468,9 +468,9 @@
     
     
     <link name='base_link'>
-      <pose frame=''>0 0 1.305 0 -0 0</pose>
+      <pose>0 0 1.305 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>1</ixx>
@@ -482,7 +482,7 @@
         </inertia>
       </inertial>
       <visual name='base_link_visual'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
 		  <box>
 		     <size>0.05 0.05 0.1 </size>
@@ -496,7 +496,7 @@
         </material>
       </visual>
         <collision name='base_link_collision'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
 		  <box>
 		     <size>0.05 0.05 0.1 </size>
@@ -531,9 +531,9 @@
       </axis>
     </joint>
     <link name='left_finger'>
-      <pose frame=''>0 0.024 1.35 0 -0.05 0</pose>
+      <pose>0 0.024 1.35 0 -0.05 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -545,7 +545,7 @@
         </inertia>
       </inertial>
       <visual name='left_finger_visual'>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <geometry>
           <box>
             <size>0.01 0.01 0.08</size>
@@ -559,7 +559,7 @@
         </material>
       </visual>
        <collision name='left_finger_collision'>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <geometry>
           <box>
             <size>0.01 0.01 0.08</size>
@@ -577,9 +577,9 @@
         <lateral_friction>0.8</lateral_friction>
         <spinning_friction>.1</spinning_friction>
       </contact>
-      <pose frame=''>-0.005 0.024 1.43 0 -0.3 0</pose>
+      <pose>-0.005 0.024 1.43 0 -0.3 0</pose>
       <inertial>
-        <pose frame=''>-0.003 0 0.04 0 0 0 </pose>
+        <pose>-0.003 0 0.04 0 0 0 </pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -591,7 +591,7 @@
         </inertia>
       </inertial>
       <visual name='left_finger_base_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -606,7 +606,7 @@
         </material>
       </visual>
       <collision name='left_finger_base_collision'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -639,9 +639,9 @@
         <lateral_friction>0.8</lateral_friction>
         <spinning_friction>.1</spinning_friction>
       </contact>
-      <pose frame=''>-0.02 0.024 1.49 0 0.2 0</pose>
+      <pose>-0.02 0.024 1.49 0 0.2 0</pose>
       <inertial>
-        <pose frame=''>-0.005 0 0.026 0 0 0 </pose>
+        <pose>-0.005 0 0.026 0 0 0 </pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -653,7 +653,7 @@
         </inertia>
       </inertial>
       <visual name='left_finger_tip_visual'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -668,7 +668,7 @@
         </material>
       </visual>
       <collision name='left_finger_tip_collision'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -701,9 +701,9 @@
         <lateral_friction>0.8</lateral_friction>
         <spinning_friction>.1</spinning_friction>
       </contact>
-      <pose frame=''>0 0.024 1.35 0 0.05 0</pose>
+      <pose>0 0.024 1.35 0 0.05 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -715,7 +715,7 @@
         </inertia>
       </inertial>
       <visual name='right_finger_visual'>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <geometry>
           <box>
             <size>0.01 0.01 0.08</size>
@@ -729,7 +729,7 @@
         </material>
       </visual>
       <collision name='right_finger_collision'>
-        <pose frame=''>0 0 0.04 0 0 0</pose>
+        <pose>0 0 0.04 0 0 0</pose>
         <geometry>
           <box>
             <size>0.01 0.01 0.08</size>
@@ -752,9 +752,9 @@
         <lateral_friction>0.8</lateral_friction>
         <spinning_friction>.1</spinning_friction>
       </contact>
-      <pose frame=''>0.005 0.024 1.43 0 0.3 0</pose>
+      <pose>0.005 0.024 1.43 0 0.3 0</pose>
       <inertial>
-        <pose frame=''>0.003 0 0.04 0 0 0 </pose>
+        <pose>0.003 0 0.04 0 0 0 </pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -766,7 +766,7 @@
         </inertia>
       </inertial>
       <visual name='right_finger_base_visual'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -781,7 +781,7 @@
         </material>
       </visual>
       <collision name='right_finger_base_collision'>
-        <pose frame=''>0 0 0 0 0 0 </pose>
+        <pose>0 0 0 0 0 0 </pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -814,9 +814,9 @@
         <lateral_friction>0.8</lateral_friction>
         <spinning_friction>.1</spinning_friction>
       </contact>
-      <pose frame=''>0.02 0.024 1.49 0 -0.2 0</pose>
+      <pose>0.02 0.024 1.49 0 -0.2 0</pose>
       <inertial>
-        <pose frame=''>0.005 0 0.026 0 0 0 </pose>
+        <pose>0.005 0 0.026 0 0 0 </pose>
         <mass>0.2</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -828,7 +828,7 @@
         </inertia>
       </inertial>
       <visual name='right_finger_visual'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>
@@ -843,7 +843,7 @@
         </material>
       </visual>
       <collision name='right_finger_tip_collision'>
-        <pose frame=''>0 0 0 0 0 0</pose>
+        <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1 </scale>

data/kuka_iiwa/kuka_world.sdf

@@ -6,9 +6,9 @@
       <child>lbr_iiwa_link_0</child>
     </joint>
     <link name='lbr_iiwa_link_0'>
-      <pose frame=''>0 0 0 0 -0 0</pose>
+      <pose>0 0 0 0 -0 0</pose>
       <inertial>
-        <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+        <pose>-0.1 0 0.07 0 -0 0</pose>
         <mass>0.01</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -20,7 +20,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_0_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -29,7 +29,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_0_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -39,9 +39,9 @@
       </visual>
     </link>
     <link name='lbr_iiwa_link_1'>
-      <pose frame=''>0 0 0.1575 0 -0 0</pose>
+      <pose>0 0 0.1575 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+        <pose>0 -0.03 0.12 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -53,7 +53,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_1_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -62,7 +62,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_1_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -92,9 +92,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_2'>
-      <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+      <pose>0 0 0.36 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+        <pose>0.0003 0.059 0.042 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -106,7 +106,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_2_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -115,7 +115,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_2_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -145,9 +145,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_3'>
-      <pose frame=''>0 -0 0.5645 0 0 0</pose>
+      <pose>0 -0 0.5645 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+        <pose>0 0.03 0.13 0 -0 0</pose>
         <mass>3</mass>
         <inertia>
           <ixx>0.08</ixx>
@@ -159,7 +159,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_3_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -168,7 +168,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_3_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -198,9 +198,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_4'>
-      <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+      <pose>0 -0 0.78 1.5708 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+        <pose>0 0.067 0.034 0 -0 0</pose>
         <mass>2.7</mass>
         <inertia>
           <ixx>0.03</ixx>
@@ -212,7 +212,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_4_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -221,7 +221,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_4_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -251,9 +251,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_5'>
-      <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+      <pose>0 -0 0.9645 0 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+        <pose>0.0001 0.021 0.076 0 -0 0</pose>
         <mass>1.7</mass>
         <inertia>
           <ixx>0.02</ixx>
@@ -265,7 +265,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_5_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -274,7 +274,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_5_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -304,9 +304,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_6'>
-      <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+      <pose>0 0 1.18 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+        <pose>0 0.0006 0.0004 0 -0 0</pose>
         <mass>1.8</mass>
         <inertia>
           <ixx>0.005</ixx>
@@ -318,7 +318,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_6_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -327,7 +327,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_6_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -357,9 +357,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_7'>
-      <pose frame=''>0 0 1.261 0 0 0</pose>
+      <pose>0 0 1.261 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.02 0 -0 0</pose>
+        <pose>0 0 0.02 0 -0 0</pose>
         <mass>0.3</mass>
         <inertia>
           <ixx>0.001</ixx>
@@ -371,7 +371,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_7_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -380,7 +380,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_7_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>

data/kuka_iiwa/model.sdf

@@ -1,11 +1,11 @@
 <sdf version='1.6'>
 <world name='default'>
   <model name='lbr_iiwa'>
-  <pose frame=''>0 -2.3 0.7 0 0 0</pose>
+  <pose>0 -2.3 0.7 0 0 0</pose>
     <link name='lbr_iiwa_link_0'>
-      <pose frame=''>0 0 0 0 -0 0</pose>
+      <pose>0 0 0 0 -0 0</pose>
       <inertial>
-        <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+        <pose>-0.1 0 0.07 0 -0 0</pose>
         <mass>0</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -17,7 +17,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_0_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -26,7 +26,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_0_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -42,9 +42,9 @@
       </visual>
     </link>
     <link name='lbr_iiwa_link_1'>
-      <pose frame=''>0 0 0.1575 0 -0 0</pose>
+      <pose>0 0 0.1575 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+        <pose>0 -0.03 0.12 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -56,7 +56,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_1_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -65,7 +65,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_1_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -101,9 +101,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_2'>
-      <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+      <pose>0 0 0.36 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+        <pose>0.0003 0.059 0.042 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -115,7 +115,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_2_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -124,7 +124,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_2_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -160,9 +160,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_3'>
-      <pose frame=''>0 -0 0.5645 0 0 0</pose>
+      <pose>0 -0 0.5645 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+        <pose>0 0.03 0.13 0 -0 0</pose>
         <mass>3</mass>
         <inertia>
           <ixx>0.08</ixx>
@@ -174,7 +174,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_3_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -183,7 +183,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_3_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -219,9 +219,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_4'>
-      <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+      <pose>0 -0 0.78 1.5708 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+        <pose>0 0.067 0.034 0 -0 0</pose>
         <mass>2.7</mass>
         <inertia>
           <ixx>0.03</ixx>
@@ -233,7 +233,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_4_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -242,7 +242,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_4_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -278,9 +278,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_5'>
-      <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+      <pose>0 -0 0.9645 0 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+        <pose>0.0001 0.021 0.076 0 -0 0</pose>
         <mass>1.7</mass>
         <inertia>
           <ixx>0.02</ixx>
@@ -292,7 +292,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_5_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -301,7 +301,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_5_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -337,9 +337,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_6'>
-      <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+      <pose>0 0 1.18 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+        <pose>0 0.0006 0.0004 0 -0 0</pose>
         <mass>1.8</mass>
         <inertia>
           <ixx>0.005</ixx>
@@ -351,7 +351,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_6_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -360,7 +360,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_6_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -396,9 +396,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_7'>
-      <pose frame=''>0 0 1.261 0 0 0</pose>
+      <pose>0 0 1.261 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.02 0 -0 0</pose>
+        <pose>0 0 0.02 0 -0 0</pose>
         <mass>0.3</mass>
         <inertia>
           <ixx>0.001</ixx>
@@ -410,7 +410,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_7_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -419,7 +419,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_7_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>

data/kuka_iiwa/model2.sdf

@@ -2,9 +2,9 @@
 <world name='default'>
   <model name='lbr_iiwa'>
     <link name='lbr_iiwa_link_0'>
-      <pose frame=''>0 0 0 0 -0 0</pose>
+      <pose>0 0 0 0 -0 0</pose>
       <inertial>
-        <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+        <pose>-0.1 0 0.07 0 -0 0</pose>
         <mass>0</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -16,7 +16,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_0_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -25,7 +25,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_0_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -35,9 +35,9 @@
       </visual>
     </link>
     <link name='lbr_iiwa_link_1'>
-      <pose frame=''>0 0 0.1575 0 -0 0</pose>
+      <pose>0 0 0.1575 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+        <pose>0 -0.03 0.12 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -49,7 +49,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_1_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -58,7 +58,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_1_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -88,9 +88,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_2'>
-      <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+      <pose>0 0 0.36 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+        <pose>0.0003 0.059 0.042 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -102,7 +102,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_2_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -111,7 +111,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_2_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -141,9 +141,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_3'>
-      <pose frame=''>0 -0 0.5645 0 0 0</pose>
+      <pose>0 -0 0.5645 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+        <pose>0 0.03 0.13 0 -0 0</pose>
         <mass>3</mass>
         <inertia>
           <ixx>0.08</ixx>
@@ -155,7 +155,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_3_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -164,7 +164,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_3_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -194,9 +194,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_4'>
-      <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+      <pose>0 -0 0.78 1.5708 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+        <pose>0 0.067 0.034 0 -0 0</pose>
         <mass>2.7</mass>
         <inertia>
           <ixx>0.03</ixx>
@@ -208,7 +208,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_4_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -217,7 +217,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_4_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -247,9 +247,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_5'>
-      <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+      <pose>0 -0 0.9645 0 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+        <pose>0.0001 0.021 0.076 0 -0 0</pose>
         <mass>1.7</mass>
         <inertia>
           <ixx>0.02</ixx>
@@ -261,7 +261,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_5_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -270,7 +270,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_5_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -300,9 +300,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_6'>
-      <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+      <pose>0 0 1.18 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+        <pose>0 0.0006 0.0004 0 -0 0</pose>
         <mass>1.8</mass>
         <inertia>
           <ixx>0.005</ixx>
@@ -314,7 +314,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_6_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -323,7 +323,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_6_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -353,9 +353,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_7'>
-      <pose frame=''>0 0 1.261 0 0 0</pose>
+      <pose>0 0 1.261 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.02 0 -0 0</pose>
+        <pose>0 0 0.02 0 -0 0</pose>
         <mass>0.3</mass>
         <inertia>
           <ixx>0.001</ixx>
@@ -367,7 +367,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_7_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -376,7 +376,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_7_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -408,11 +408,11 @@
   </model>
   
   <model name='lbr_iiwa'>
-    <pose frame=''>2 2 0 0 -0 0</pose>
+    <pose>2 2 0 0 -0 0</pose>
     <link name='lbr_iiwa_link_0'>
-      <pose frame=''>0 0 0 0 -0 0</pose>
+      <pose>0 0 0 0 -0 0</pose>
       <inertial>
-        <pose frame=''>-0.1 0 0.07 0 -0 0</pose>
+        <pose>-0.1 0 0.07 0 -0 0</pose>
         <mass>0</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -424,7 +424,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_0_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -433,7 +433,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_0_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -443,9 +443,9 @@
       </visual>
     </link>
     <link name='lbr_iiwa_link_1'>
-      <pose frame=''>0 0 0.1575 0 -0 0</pose>
+      <pose>0 0 0.1575 0 -0 0</pose>
       <inertial>
-        <pose frame=''>0 -0.03 0.12 0 -0 0</pose>
+        <pose>0 -0.03 0.12 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.1</ixx>
@@ -457,7 +457,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_1_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -466,7 +466,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_1_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -496,9 +496,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_2'>
-      <pose frame=''>0 0 0.36 1.5708 -0 -3.14159</pose>
+      <pose>0 0 0.36 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0003 0.059 0.042 0 -0 0</pose>
+        <pose>0.0003 0.059 0.042 0 -0 0</pose>
         <mass>4</mass>
         <inertia>
           <ixx>0.05</ixx>
@@ -510,7 +510,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_2_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -519,7 +519,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_2_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -549,9 +549,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_3'>
-      <pose frame=''>0 -0 0.5645 0 0 0</pose>
+      <pose>0 -0 0.5645 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.03 0.13 0 -0 0</pose>
+        <pose>0 0.03 0.13 0 -0 0</pose>
         <mass>3</mass>
         <inertia>
           <ixx>0.08</ixx>
@@ -563,7 +563,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_3_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -572,7 +572,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_3_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -602,9 +602,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_4'>
-      <pose frame=''>0 -0 0.78 1.5708 0 0</pose>
+      <pose>0 -0 0.78 1.5708 0 0</pose>
       <inertial>
-        <pose frame=''>0 0.067 0.034 0 -0 0</pose>
+        <pose>0 0.067 0.034 0 -0 0</pose>
         <mass>2.7</mass>
         <inertia>
           <ixx>0.03</ixx>
@@ -616,7 +616,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_4_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -625,7 +625,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_4_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -655,9 +655,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_5'>
-      <pose frame=''>0 -0 0.9645 0 -0 -3.14159</pose>
+      <pose>0 -0 0.9645 0 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0.0001 0.021 0.076 0 -0 0</pose>
+        <pose>0.0001 0.021 0.076 0 -0 0</pose>
         <mass>1.7</mass>
         <inertia>
           <ixx>0.02</ixx>
@@ -669,7 +669,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_5_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -678,7 +678,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_5_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -708,9 +708,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_6'>
-      <pose frame=''>0 0 1.18 1.5708 -0 -3.14159</pose>
+      <pose>0 0 1.18 1.5708 -0 -3.14159</pose>
       <inertial>
-        <pose frame=''>0 0.0006 0.0004 0 -0 0</pose>
+        <pose>0 0.0006 0.0004 0 -0 0</pose>
         <mass>1.8</mass>
         <inertia>
           <ixx>0.005</ixx>
@@ -722,7 +722,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_6_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -731,7 +731,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_6_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -761,9 +761,9 @@
       </axis>
     </joint>
     <link name='lbr_iiwa_link_7'>
-      <pose frame=''>0 0 1.261 0 0 0</pose>
+      <pose>0 0 1.261 0 0 0</pose>
       <inertial>
-        <pose frame=''>0 0 0.02 0 -0 0</pose>
+        <pose>0 0 0.02 0 -0 0</pose>
         <mass>0.3</mass>
         <inertia>
           <ixx>0.001</ixx>
@@ -775,7 +775,7 @@
         </inertia>
       </inertial>
       <collision name='lbr_iiwa_link_7_collision'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>
@@ -784,7 +784,7 @@
         </geometry>
       </collision>
       <visual name='lbr_iiwa_link_7_visual'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <geometry>
           <mesh>
             <scale>1 1 1</scale>

data/stadium.sdf

@@ -3,7 +3,7 @@
 	<gravity>0 0 -9.8</gravity>
 		<model name='roboschool/models_outdoor/stadium/part0.obj'>
 			<static>1</static>
-			<pose frame=''>0 0 0 0 0 0</pose>
+			<pose>0 0 0 0 0 0</pose>
 			<link name='link_d0'>
 			<inertial>
 			<mass>0</mass>
@@ -34,7 +34,7 @@
 			</model>
 		<model name='roboschool/models_outdoor/stadium/part1.obj'>
 			<static>1</static>
-			<pose frame=''>0 0 0 0 0 0</pose>
+			<pose>0 0 0 0 0 0</pose>
 			<link name='link_d1'>
 			<inertial>
 			<mass>0</mass>
@@ -73,7 +73,7 @@
 			</model>
 		<model name='part2.obj'>
 			<static>1</static>
-			<pose frame=''>0 0 0 0 0 0</pose>
+			<pose>0 0 0 0 0 0</pose>
 			<link name='link_d2'>
 			<inertial>
 			<mass>0</mass>

data/threecubes/newsdf.sdf

@@ -3,7 +3,7 @@
 	<gravity>0 0 -9.8</gravity>
 		<model name='part0.obj'>
 			<static>1</static>
-			<pose frame=''>0 0 0 0 0 0</pose>
+			<pose>0 0 0 0 0 0</pose>
 			<link name='link_d0'>
 			<inertial>
 			<mass>0</mass>
@@ -42,7 +42,7 @@
 			</model>
 		<model name='part1.obj'>
 			<static>1</static>
-			<pose frame=''>0 0 0 0 0 0</pose>
+			<pose>0 0 0 0 0 0</pose>
 			<link name='link_d1'>
 			<inertial>
 			<mass>0</mass>
@@ -81,7 +81,7 @@
 			</model>
 		<model name='part2.obj'>
 			<static>1</static>
-			<pose frame=''>0 0 0 0 0 0</pose>
+			<pose>0 0 0 0 0 0</pose>
 			<link name='link_d2'>
 			<inertial>
 			<mass>0</mass>

data/two_cubes.sdf

@@ -3,7 +3,7 @@
     <light name='sun' type='directional'>
      <yunfei>99.2</yunfei> 
 	<cast_shadows>1</cast_shadows>
-      <pose frame=''>0 0 10 0 -0 0</pose>
+      <pose>0 0 10 0 -0 0</pose>
       <diffuse>0.8 0.8 0.8 1</diffuse>
       <specular>0.2 0.2 0.2 1</specular>
       <attenuation>
@@ -87,7 +87,7 @@
     </spherical_coordinates>
     <model name='unit_box_0'>
  <static>1</static>
-      <pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
+      <pose>0.512455 -1.58317 0.5 0 -0 0</pose>
       <link name='unit_box_0::link'>
       
         <inertial>
@@ -142,7 +142,7 @@
       </link>
     </model>
     <model name='unit_box_0_clone'>
-      <pose frame=''>0.105158 -4.55002 1.499995 -2.89297 -0.988287 -3.14159</pose>
+      <pose>0.105158 -4.55002 1.499995 -2.89297 -0.988287 -3.14159</pose>
       <link name='unit_box_0::link'>
         <inertial>
           <mass>1</mass>
@@ -197,42 +197,42 @@
       <wall_time>1462824251 956472000</wall_time>
       <iterations>0</iterations>
       <model name='ground_plane'>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose>0 0 0 0 -0 0</pose>
         <scale>1 1 1</scale>
         <link name='link'>
-          <pose frame=''>0 0 0 0 -0 0</pose>
+          <pose>0 0 0 0 -0 0</pose>
           <velocity>0 0 0 0 -0 0</velocity>
           <acceleration>0 0 0 0 -0 0</acceleration>
           <wrench>0 0 0 0 -0 0</wrench>
         </link>
       </model>
       <model name='unit_box_0'>
-        <pose frame=''>0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159</pose>
+        <pose>0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159</pose>
         <scale>1 1 1</scale>
         <link name='unit_box_0::link'>
-          <pose frame=''>0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159</pose>
+          <pose>0.223196 -1.84719 0.499995 -2.89297 -0.988287 -3.14159</pose>
           <velocity>0.004896 3e-06 -0.004891 -6e-06 0.009793 -0</velocity>
           <acceleration>0.010615 0.006191 -9.78231 -0.012424 0.021225 -1.8e-05</acceleration>
           <wrench>0.010615 0.006191 -9.78231 0 -0 0</wrench>
         </link>
       </model>
       <model name='unit_box_0_clone'>
-        <pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
+        <pose>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
         <scale>1 1 1</scale>
         <link name='unit_box_0::link'>
-          <pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
+          <pose>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
           <velocity>0 0 0 0 -0 0</velocity>
           <acceleration>0 0 0 0 -0 0</acceleration>
           <wrench>0 0 0 0 -0 0</wrench>
         </link>
       </model>
       <light name='sun'>
-        <pose frame=''>0 0 10 0 -0 0</pose>
+        <pose>0 0 10 0 -0 0</pose>
       </light>
     </state>
     <gui fullscreen='0'>
       <camera name='user_camera'>
-        <pose frame=''>8.0562 -8.87312 3.07529 0 0.205021 2.5208</pose>
+        <pose>8.0562 -8.87312 3.07529 0 0.205021 2.5208</pose>
         <view_controller>orbit</view_controller>
         <projection_type>perspective</projection_type>
       </camera>

examples/BasicDemo/CMakeLists.txt

@@ -84,7 +84,8 @@ ELSE(WIN32)
                 ADD_DEFINITIONS("-DGLEW_STATIC")
                 ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
 
-                LINK_LIBRARIES(  pthread ${DL} )
+                FIND_PACKAGE(Threads)
+                LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
         ENDIF(APPLE)
 ENDIF(WIN32)
 

examples/Benchmarks/BenchmarkDemo.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Benchmarks/BenchmarkDemo.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -52,6 +52,8 @@ struct GUIHelperInterface
 	virtual int getShapeIndexFromInstance(int instanceUid) { return -1; }
 	virtual void replaceTexture(int shapeIndex, int textureUid) {}
 	virtual void removeTexture(int textureUid) {}
+	virtual void setBackgroundColor(const double rgbBackground[3]) {}
+	
 
 	virtual Common2dCanvasInterface* get2dCanvasInterface() = 0;
 
@@ -123,6 +125,7 @@ struct GUIHelperInterface
 	virtual void dumpFramesToVideo(const char* mp4FileName){};
 	virtual void drawDebugDrawerLines(){}
 	virtual void clearLines(){}
+	virtual bool isRemoteVisualizer() { return false; }
 };
 
 ///the DummyGUIHelper does nothing, so we can test the examples without GUI/graphics (in 'console mode')

examples/CommonInterfaces/CommonRenderInterface.h

@@ -56,6 +56,8 @@ struct CommonRenderInterface
 
 	virtual void setLightPosition(const float lightPos[3]) = 0;
 	virtual void setLightPosition(const double lightPos[3]) = 0;
+	virtual void setBackgroundColor(const double rgbBackground[3]) = 0;
+	
 	virtual void setShadowMapResolution(int shadowMapResolution) = 0;
 	virtual void setShadowMapIntensity(double shadowMapIntensity) = 0;
 	

examples/Constraints/ConstraintDemo.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2015 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2015 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Constraints/ConstraintDemo.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/DeformableDemo/DeformableSelfCollision.cpp

@@ -55,7 +55,7 @@ public:
         m_dynamicsWorld->stepSimulation(deltaTime, 4, internalTimeStep);
     }
     
-    void addCloth(btVector3 origin);
+    void addCloth(const btVector3& origin);
     
     virtual void renderScene()
     {
@@ -126,7 +126,7 @@ void DeformableSelfCollision::initPhysics()
     getDeformableDynamicsWorld()->setLineSearch(false);
     m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
 }
-void DeformableSelfCollision::addCloth(btVector3 origin)
+void DeformableSelfCollision::addCloth(const btVector3& origin)
 // create a piece of cloth
 {
     const btScalar s = 0.6;

examples/ExampleBrowser/CMakeLists.txt

@@ -61,11 +61,12 @@ IF (BUILD_SHARED_LIBS)
                                 BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
                                 ${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
                  )
-  			ELSE(APPLE)
+				ELSE(APPLE)
+                 FIND_PACKAGE(Threads)
                  TARGET_LINK_LIBRARIES(
                                 BulletExampleBrowserLib Bullet3Common BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils
                                 BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK
-                                pthread ${DL}
+                                ${CMAKE_THREAD_LIBS_INIT} ${DL}
                         )
         ENDIF(APPLE)
 	ENDIF(WIN32)
@@ -116,7 +117,8 @@ ELSE(WIN32)
 		ADD_DEFINITIONS("-DGLEW_STATIC")
 		ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
 		INCLUDE_DIRECTORIES( ${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/glad )
-		LINK_LIBRARIES( pthread ${DL})
+		FIND_PACKAGE(Threads)
+		LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
 	ENDIF(APPLE)
 ENDIF(WIN32)
 
@@ -350,7 +352,8 @@ SET(BulletExampleBrowser_SRCS
   ../MultiBody/TestJointTorqueSetup.h
   ../MultiBody/InvertedPendulumPDControl.cpp
   ../MultiBody/InvertedPendulumPDControl.h
- ../MultiBody/MultiBodyConstraintFeedback.cpp
+  ../MultiBody/MultiBodyConstraintFeedback.cpp
+  ../MultiBody/KinematicMultiBodyExample.cpp
   ../SoftDemo/SoftDemo.cpp
   ../SoftDemo/SoftDemo.h
   ../DeformableDemo/DeformableContact.cpp
@@ -384,6 +387,7 @@ SET(BulletExampleBrowser_SRCS
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp
+  ../RigidBody/KinematicRigidBodyExample.cpp
   ../Constraints/TestHingeTorque.cpp
   ../Constraints/TestHingeTorque.h
   ../Constraints/ConstraintDemo.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -33,8 +33,10 @@
 #include "../MultiBody/MultiBodyConstraintFeedback.h"
 #include "../MultiBody/MultiDofDemo.h"
 #include "../MultiBody/InvertedPendulumPDControl.h"
+#include "../MultiBody/KinematicMultiBodyExample.h"
 
 #include "../RigidBody/RigidBodySoftContact.h"
+#include "../RigidBody/KinematicRigidBodyExample.h"
 #include "../VoronoiFracture/VoronoiFractureDemo.h"
 #include "../SoftDemo/SoftDemo.h"
 #include "../Constraints/ConstraintDemo.h"
@@ -145,6 +147,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Gyroscopic", "Show the Dzhanibekov effect using various settings of the gyroscopic term. You can select the gyroscopic term computation using btRigidBody::setFlags, with arguments BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT (using explicit integration, which adds energy and can lead to explosions), BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD, BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY. If you don't set any of these flags, there is no gyroscopic term used.", GyroscopicCreateFunc),
 
 		ExampleEntry(1, "Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", RigidBodySoftContactCreateFunc),
+    ExampleEntry(1, "Kinematic Body", "Let the user set the transform, the physics engine computes the velocity for one-way contact and friction interaction.", KinematicRigidBodyExampleCreateFunc),
 
 		ExampleEntry(0, "MultiBody"),
 		ExampleEntry(1, "MultiDof", "Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
@@ -154,6 +157,7 @@ static ExampleEntry gDefaultExamples[] =
 		ExampleEntry(1, "Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 		ExampleEntry(1, "Inverted Pendulum PD", "Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc),
 		ExampleEntry(1, "MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", MultiBodySoftContactCreateFunc, 0),
+    ExampleEntry(1, "Kinematic MultiBody", "Let the user set the transform, the physics engine computes the velocity for one-way contact and friction interaction.", KinematicMultiBodyExampleCreateFunc),
 
 		ExampleEntry(0, "Physics Client-Server"),
 		ExampleEntry(1, "Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.",

examples/ExampleBrowser/GL_ShapeDrawer.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/ExampleBrowser/GL_ShapeDrawer.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/ExampleBrowser/GwenGUISupport/GwenParameterInterface.cpp

@@ -1,7 +1,11 @@
 #include "GwenParameterInterface.h"
 #include "gwenInternalData.h"
 #include <cstring>
-
+#ifdef _WIN32 
+#define safe_printf _snprintf
+#else
+#define safe_printf snprintf
+#endif
 struct MyButtonEventHandler : public Gwen::Event::Handler
 {
 	Gwen::Controls::Button* m_buttonControl;
@@ -85,7 +89,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler
 		if (m_showValue)
 		{
 			char txt[1024];
-			snprintf(txt, sizeof(txt), "%s : %.3f", m_variableName, val);
+			safe_printf(txt, sizeof(txt), "%s : %.3f", m_variableName, val);
 			m_label->SetText(txt);
 		}
 	}
@@ -231,7 +235,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
 	}
 	pSlider->SetValue(*params.m_paramValuePointer);  //dimensions[i] );
 	char labelName[1024];
-	snprintf(labelName, sizeof(labelName), "%s", params.m_name);  //axisNames[0]);
+	safe_printf(labelName, sizeof(labelName), "%s", params.m_name);  //axisNames[0]);
 	MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName, label, pSlider, params.m_paramValuePointer, params.m_callback, params.m_userPointer);
 	handler->m_showValue = params.m_showValues;
 	m_paramInternalData->m_sliderEventHandlers.push_back(handler);

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -236,13 +236,19 @@ OpenGLGuiHelper::OpenGLGuiHelper(CommonGraphicsApp* glApp, bool useOpenGL2)
 	m_data->m_debugDraw = 0;
 }
 
-OpenGLGuiHelper::~OpenGLGuiHelper()
+
+	OpenGLGuiHelper::~OpenGLGuiHelper()
 {
 	delete m_data->m_debugDraw;
 
 	delete m_data;
 }
 
+void OpenGLGuiHelper::setBackgroundColor(const double rgbBackground[3])
+{
+	this->getRenderInterface()->setBackgroundColor(rgbBackground);
+}
+
 struct CommonRenderInterface* OpenGLGuiHelper::getRenderInterface()
 {
 	return m_data->m_glApp->m_renderer;

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -36,7 +36,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface
 	virtual int getShapeIndexFromInstance(int instanceUid);
 	virtual void replaceTexture(int shapeIndex, int textureUid);
 	virtual void updateShape(int shapeIndex, float* vertices, int numVertices);
-
+	virtual void setBackgroundColor(const double rgbBackground[3]);
 	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape);
 
 	virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld);

examples/ExampleBrowser/premake4.lua

@@ -196,7 +196,9 @@ project "App_BulletExampleBrowser"
 		"../MultiBody/MultiBodySoftContact.cpp",
 		"../MultiBody/MultiBodyConstraintFeedback.cpp",
 		"../MultiBody/InvertedPendulumPDControl.cpp",
+		"../MultiBody/KinematicMultiBodyExample.cpp",
 		"../RigidBody/RigidBodySoftContact.cpp",
+		"../RigidBody/KinematicRigidBodyExample.cpp",
 		"../ThirdPartyLibs/stb_image/stb_image.cpp",
 		"../ThirdPartyLibs/Wavefront/tiny_obj_loader.*",
 		"../GyroscopicDemo/GyroscopicSetup.cpp",

examples/ForkLift/ForkLiftDemo.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/ForkLift/ForkLiftDemo.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/FractureDemo/FractureDemo.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2011 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2011 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/FractureDemo/FractureDemo.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/HelloWorld/HelloWorld.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2007 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2007 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Importers/ImportBsp/BspConverter.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Importers/ImportBsp/BspConverter.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Importers/ImportBsp/ImportBspExample.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Importers/ImportBsp/ImportBspExample.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp

@@ -33,6 +33,10 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc
 {
 	btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
 	rbci.m_startWorldTransform = initialWorldTrans;
+	btScalar sleep_threshold = btScalar(0.22360679775);//sqrt(0.05) to be similar to btMultiBody (SLEEP_THRESHOLD)
+	rbci.m_angularSleepingThreshold = sleep_threshold;
+	rbci.m_linearSleepingThreshold = sleep_threshold;
+	
 	btRigidBody* body = new btRigidBody(rbci);
 	if (m_rigidBody == 0)
 	{

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -199,7 +199,15 @@ btScalar tmpUrdfScaling = 2;
 btTransform ConvertURDF2BulletInternal(
 	const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
 	URDF2BulletCachedData& cache, int urdfLinkIndex,
-	const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1,
+	const btTransform& parentTransformInWorldSpace, 
+	
+#ifdef USE_DISCRETE_DYNAMICS_WORLD
+	btDiscreteDynamicsWorld* world1,
+#else
+	btMultiBodyDynamicsWorld* world1,
+#endif
+
+
 	bool createMultiBody, const char* pathPrefix,
 	int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapesIn, UrdfVisualShapeCache* cachedLinkGraphicsShapesOut, bool recursive)
 {
@@ -505,6 +513,7 @@ btTransform ConvertURDF2BulletInternal(
 					creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
 					if (createMultiBody)
 					{
+#ifndef USE_DISCRETE_DYNAMICS_WORLD
 						cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
 															   parentRotToThis, quatRotate(offsetInB.getRotation(), 
 																   jointAxisInJointSpace), 
@@ -519,8 +528,10 @@ btTransform ConvertURDF2BulletInternal(
 							btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody, mbLinkIndex, jointLowerLimit, jointUpperLimit);
 							world1->addMultiBodyConstraint(con);
 						}
+#endif
 					}
 					else
+
 					{
 						btGeneric6DofSpring2Constraint* dof6 = 0;
 						if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit)
@@ -560,6 +571,7 @@ btTransform ConvertURDF2BulletInternal(
 
 					if (createMultiBody)
 					{
+#ifndef USE_DISCRETE_DYNAMICS_WORLD
 						cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
 																parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(),  //parent2joint.getOrigin(),
 																-offsetInB.getOrigin(),
@@ -574,6 +586,7 @@ btTransform ConvertURDF2BulletInternal(
 							world1->addMultiBodyConstraint(con);
 						}
 						//printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
+#endif
 					}
 					else
 					{
@@ -775,10 +788,18 @@ bool MyIntCompareFunc(childParentIndex a, childParentIndex b)
 }
 
 
+	
+
+
 void ConvertURDF2Bullet(
 	const URDFImporterInterface& u2b, MultiBodyCreationInterface& creation,
 	const btTransform& rootTransformInWorldSpace,
+
+#ifdef USE_DISCRETE_DYNAMICS_WORLD
+	btDiscreteDynamicsWorld* world1,
+#else
 	btMultiBodyDynamicsWorld* world1,
+#endif
 	bool createMultiBody, const char* pathPrefix, int flags, UrdfVisualShapeCache* cachedLinkGraphicsShapes)
 {
 	URDF2BulletCachedData cache;
@@ -829,7 +850,7 @@ void ConvertURDF2Bullet(
 	{
 		*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
 	}
-
+#ifndef USE_DISCRETE_DYNAMICS_WORLD
 	if (world1 && cache.m_bulletMultiBody)
 	{
 		B3_PROFILE("Post process");
@@ -855,4 +876,5 @@ void ConvertURDF2Bullet(
 
 		world1->addMultiBody(mb);
 	}
+	#endif
 }

examples/Importers/ImportURDFDemo/URDF2Bullet.h

@@ -8,6 +8,7 @@
 class btVector3;
 class btTransform;
 class btMultiBodyDynamicsWorld;
+class btDiscreteDynamicsWorld;
 class btTransform;
 
 class URDFImporterInterface;
@@ -20,7 +21,18 @@ struct UrdfVisualShapeCache
 	btAlignedObjectArray<UrdfMaterialColor> m_cachedUrdfLinkColors;
 	btAlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices;
 };
+//#define USE_DISCRETE_DYNAMICS_WORLD
+#ifdef USE_DISCRETE_DYNAMICS_WORLD
+	void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
+						MultiBodyCreationInterface& creationCallback,
+						const btTransform& rootTransformInWorldSpace,
+						btDiscreteDynamicsWorld* world,
+						bool createMultiBody,
+						const char* pathPrefix,
+						int flags = 0,
+						UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0);
 
+#else
 void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
 						MultiBodyCreationInterface& creationCallback,
 						const btTransform& rootTransformInWorldSpace,
@@ -29,5 +41,5 @@ void ConvertURDF2Bullet(const URDFImporterInterface& u2b,
 						const char* pathPrefix,
 						int flags = 0,
 						UrdfVisualShapeCache* cachedLinkGraphicsShapes = 0);
-
+#endif
 #endif  //_URDF2BULLET_H

examples/Importers/ImportURDFDemo/UrdfParser.cpp

@@ -94,7 +94,8 @@ static void ParseUserData(const XMLElement* element, btHashMap<btHashString,
 			if (!key_attr) {
 				logger->reportError("User data tag must have a key attribute.");
 			}
-			user_data.insert(key_attr, user_data_xml->GetText());
+			const char* text = user_data_xml->GetText();
+			user_data.insert(key_attr, text ? text : "");
 		}
 	}
 }
@@ -1132,6 +1133,7 @@ bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config,
 	if (!i)
 	{
 		logger->reportError("expected an inertial element");
+		return false;
 	}
 	UrdfInertia inertia;
 	if (!parseInertia(inertia, i, logger))
@@ -1517,8 +1519,9 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, XMLElement* config, ErrorLogger* l
 			XMLElement* axis_xml = config->FirstChildElement("axis");
 			if (!axis_xml)
 			{
-				logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
-				logger->reportWarning(joint.m_name.c_str());
+				std::string msg("urdfdom: no axis element for Joint, defaulting to (1,0,0) axis");
+				msg = msg + " " + joint.m_name + "\n";
+				logger->reportWarning(msg.c_str());
 				joint.m_localJointAxis.setValue(1, 0, 0);
 			}
 			else
@@ -1584,8 +1587,9 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, XMLElement* config, ErrorLogger* l
 			XMLElement* axis_xml = config->FirstChildElement("axis");
 			if (!axis_xml)
 			{
-				logger->reportWarning("urdfdom: no axis elemement for Joint, defaulting to (1,0,0) axis");
-				logger->reportWarning(joint.m_name.c_str());
+				std::string msg("urdfdom: no axis element for Joint, defaulting to (1,0,0) axis");
+				msg = msg + " " + joint.m_name + "\n";
+				logger->reportWarning(msg.c_str());
 				joint.m_localJointAxis.setValue(1, 0, 0);
 			}
 			else

examples/MultiBody/KinematicMultiBodyExample.cpp

@@ -0,0 +1,201 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "KinematicMultiBodyExample.h"
+//#define USE_MOTIONSTATE 1
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btTransformUtil.h"
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Z 5
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../OpenGLWindow/ShapeData.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+namespace {
+
+void kinematicPreTickCallback(btDynamicsWorld* world, btScalar deltaTime)
+{
+	btMultiBody* groundBody = (btMultiBody*)world->getWorldUserInfo();
+	btTransform predictedTrans;
+	btVector3 linearVelocity(0, 0, 0);
+	btVector3 angularVelocity(0, 0.1, 0);
+	btTransformUtil::integrateTransform(groundBody->getBaseWorldTransform(), linearVelocity, angularVelocity, deltaTime, predictedTrans);
+	groundBody->setBaseWorldTransform(predictedTrans);
+	groundBody->setBaseVel(linearVelocity);
+	groundBody->setBaseOmega(angularVelocity);
+
+	static float time = 0.0;
+	time += deltaTime;
+	double old_joint_pos = groundBody->getJointPos(0);
+	double joint_pos = 0.5 * sin(time * 3.0 - 0.3);
+	double joint_vel = (joint_pos - old_joint_pos) / deltaTime;
+	groundBody->setJointPosMultiDof(0, &joint_pos);
+	groundBody->setJointVelMultiDof(0, &joint_vel);
+}
+
+struct KinematicMultiBodyExample : public CommonMultiBodyBase
+{
+	btMultiBody* m_groundBody;
+
+	KinematicMultiBodyExample(struct GUIHelperInterface* helper)
+		: CommonMultiBodyBase(helper),
+		  m_groundBody(0)
+	{
+	}
+
+	virtual void stepSimulation(float deltaTime)
+	{
+		if (m_dynamicsWorld)
+		{
+			m_dynamicsWorld->stepSimulation(deltaTime);
+		}
+	}
+
+	virtual ~KinematicMultiBodyExample() {}
+	virtual void initPhysics();
+	void resetCamera()
+	{
+		float dist = 4;
+		float pitch = -30;
+		float yaw = 50;
+		float targetPos[3] = {0, 0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+void KinematicMultiBodyExample::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	createEmptyDynamicsWorld();
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+	if (m_dynamicsWorld->getDebugDrawer())
+		m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints);
+
+	///create a kinematic multibody
+	btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10.), btScalar(0.1), btScalar(10.)));
+	m_collisionShapes.push_back(groundShape);
+
+	btBoxShape* secondLevelShape = createBoxShape(btVector3(btScalar(0.5), btScalar(0.1), btScalar(0.5)));
+	m_collisionShapes.push_back(secondLevelShape);
+
+	{
+		bool floating = false;
+		int numLinks = 1;
+		bool canSleep = false;
+		btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+		float baseMass = 1.f;
+		btVector3 secondLevelInertiaDiag(0.f, 0.f, 0.f);
+		float secondLevelMass = 0.1f;
+
+		if (baseMass)
+		{
+			btCollisionShape* pTempBox = new btBoxShape(btVector3(10, 10, 10));
+			pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
+			delete pTempBox;
+		}
+		if (secondLevelMass)
+		{
+			btCollisionShape* pTempBox = new btBoxShape(btVector3(0.5, 0.5, 0.5));
+			pTempBox->calculateLocalInertia(secondLevelMass, secondLevelInertiaDiag);
+			delete pTempBox;
+		}
+		btTransform startTransform;
+		startTransform.setIdentity();
+
+		m_groundBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
+		m_groundBody->setBasePos(startTransform.getOrigin());
+		m_groundBody->setWorldToBaseRot(startTransform.getRotation());
+
+		//init the child link - second level.
+		btVector3 hingeJointAxis(0, 1, 0);
+		m_groundBody->setupRevolute(0, secondLevelMass, secondLevelInertiaDiag, -1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, btVector3(0, 0.5, 0), btVector3(0, 0, 0), true);
+
+		m_groundBody->finalizeMultiDof();
+		m_dynamicsWorld->addMultiBody(m_groundBody);
+
+		// add collision geometries
+		bool isDynamic = false; // Kinematic is not treated as dynamic here.
+		int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+		int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+		btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(m_groundBody, -1);
+		col->setCollisionShape(groundShape);
+		m_dynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask);  //, 2,1+2);
+		m_groundBody->setBaseCollider(col);
+		m_groundBody->setBaseDynamicType(btCollisionObject::CF_KINEMATIC_OBJECT);
+
+		btMultiBodyLinkCollider* secondLevelCol = new btMultiBodyLinkCollider(m_groundBody, 0);
+		secondLevelCol->setCollisionShape(secondLevelShape);
+		m_dynamicsWorld->addCollisionObject(secondLevelCol, collisionFilterGroup, collisionFilterMask);
+		m_groundBody->getLink(0).m_collider = secondLevelCol;
+		m_groundBody->setLinkDynamicType(0, btCollisionObject::CF_KINEMATIC_OBJECT);
+	}
+	m_dynamicsWorld->setInternalTickCallback(kinematicPreTickCallback, m_groundBody, true);
+
+	{
+		//create a few dynamic rigidbodies
+		// Re-using the same collision is better for memory usage and performance
+		btBoxShape* colShape = createBoxShape(btVector3(.1, .1, .1));
+
+		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+		m_collisionShapes.push_back(colShape);
+
+		/// Create Dynamic Objects
+		btTransform startTransform;
+		startTransform.setIdentity();
+
+		btScalar mass(1.f);
+
+		//rigidbody is dynamic if and only if mass is non zero, otherwise static
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0, 0, 0);
+		if (isDynamic)
+			colShape->calculateLocalInertia(mass, localInertia);
+
+		for (int k = 0; k < ARRAY_SIZE_Y; k++)
+		{
+			for (int i = 0; i < ARRAY_SIZE_X; i++)
+			{
+				for (int j = 0; j < ARRAY_SIZE_Z; j++)
+				{
+					startTransform.setOrigin(btVector3(
+						btScalar(0.2 * i),
+						btScalar(2 + .2 * k),
+						btScalar(0.2 * j)));
+
+					createRigidBody(mass, startTransform, colShape);
+				}
+			}
+		}
+	}
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+}
+
+CommonExampleInterface* KinematicMultiBodyExampleCreateFunc(CommonExampleOptions& options)
+{
+	return new KinematicMultiBodyExample(options.m_guiHelper);
+}
+
+B3_STANDALONE_EXAMPLE(KinematicMultiBodyExampleCreateFunc)

examples/MultiBody/KinematicMultiBodyExample.h

@@ -0,0 +1,21 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef KINEMATIC_MULTI_BODY_EXAMPLE_H
+#define KINEMATIC_MULTI_BODY_EXAMPLE_H
+
+class CommonExampleInterface* KinematicMultiBodyExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //KINEMATIC_MULTI_BODY_EXAMPLE_H

examples/MultiBody/MultiBodyConstraintFeedback.cpp

@@ -228,7 +228,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
 		m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
 
 		//////////////////////////////////////////////
-		if (0)  //numLinks > 0)
+		if (/* DISABLES CODE */ (0))  //numLinks > 0)
 		{
 			btScalar q0 = 45.f * SIMD_PI / 180.f;
 			if (!spherical)
@@ -349,7 +349,7 @@ void MultiBodyConstraintFeedbackSetup::initPhysics()
 void MultiBodyConstraintFeedbackSetup::stepSimulation(float deltaTime)
 {
 	//m_multiBody->addLinkForce(0,btVector3(100,100,100));
-	if (0)  //m_once)
+	if (/* DISABLES CODE */ (0))  //m_once)
 	{
 		m_once = false;
 		m_multiBody->addJointTorque(0, 10.0);

examples/MultiBody/MultiBodySoftContact.cpp

@@ -130,7 +130,7 @@ void MultiBodySoftContact::initPhysics()
 
 void MultiBodySoftContact::stepSimulation(float deltaTime)
 {
-	if (0)  //m_once)
+	if (/* DISABLES CODE */ (0))  //m_once)
 	{
 		m_once = false;
 		m_multiBody->addJointTorque(0, 10.0);

examples/MultiBody/TestJointTorqueSetup.cpp

@@ -230,7 +230,7 @@ void TestJointTorqueSetup::initPhysics()
 		m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
 
 		//////////////////////////////////////////////
-		if (0)  //numLinks > 0)
+		if (/* DISABLES CODE */ (0))  //numLinks > 0)
 		{
 			btScalar q0 = 45.f * SIMD_PI / 180.f;
 			if (!spherical)
@@ -356,7 +356,7 @@ void TestJointTorqueSetup::initPhysics()
 void TestJointTorqueSetup::stepSimulation(float deltaTime)
 {
 	//m_multiBody->addLinkForce(0,btVector3(100,100,100));
-	if (0)  //m_once)
+	if (/* DISABLES CODE */ (0))  //m_once)
 	{
 		m_once = false;
 		m_multiBody->addJointTorque(0, 10.0);

examples/MultiBodyBaseline/MultiBodyBaseline.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
@@ -354,5 +354,3 @@ class CommonExampleInterface* MultiBodyBaselineCreateFunc(struct CommonExampleOp
 {
 	return new MultiBodyBaseline(options.m_guiHelper);
 }
-
-

examples/MultiBodyBaseline/MultiBodyBaseline.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/MultiThreadedDemo/CommonRigidBodyMTBase.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/MultiThreadedDemo/MultiThreadedDemo.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/MultiThreadedDemo/MultiThreadedDemo.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/OpenGLWindow/EGLOpenGLWindow.cpp

@@ -33,6 +33,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <cstdlib>
 
 #include "OpenGLInclude.h"
 
@@ -124,6 +125,28 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
 	{
 		printf("eglQueryDevicesEXT Failed.\n");
 		m_data->egl_display = EGL_NO_DISPLAY;
+	} else
+	{
+		// default case, should always happen (for future compatibility)
+		if (m_data->m_renderDevice == -1)
+		{
+			// check env variable
+			const char* env_p = std::getenv("EGL_VISIBLE_DEVICES");
+
+			// variable is set
+			if(env_p != NULL)
+			{
+				m_data->m_renderDevice = std::atoi(env_p);
+				fprintf(stderr, "EGL device choice: %d of %d (from EGL_VISIBLE_DEVICES)\n", m_data->m_renderDevice, num_devices);
+
+            } else {
+                fprintf(stderr, "EGL device choice: %d of %d.\n", m_data->m_renderDevice, num_devices);
+            } // else leave with -1
+
+		} else
+		{
+			fprintf(stderr, "EGL device choice: %d of %d.\n", m_data->m_renderDevice, num_devices);
+		}
 	}
 	// Query EGL Screens
 	if (m_data->m_renderDevice == -1)
@@ -179,7 +202,7 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
 
 	if (!eglInitialize(m_data->egl_display, NULL, NULL))
 	{
-		fprintf(stderr, "Unable to initialize EGL\n");
+		fprintf(stderr, "eglInitialize() failed with error: %x\n", eglGetError());
 		exit(EXIT_FAILURE);
 	}
 

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -397,6 +397,16 @@ void GLInstancingRenderer::removeAllInstances()
 	m_graphicsInstances.clear();
 	m_data->m_publicGraphicsInstances.exitHandles();
 	m_data->m_publicGraphicsInstances.initHandles();
+
+#if 0
+	//todo: cannot release ALL textures, since some handles are still kept, and it would cause a crash
+	for (int i=0;i<m_data->m_textureHandles.size();i++)
+	{
+		InternalTextureHandle& h = m_data->m_textureHandles[i];
+		glDeleteTextures(1, &h.m_glTexture);
+	}
+	m_data->m_textureHandles.clear();
+#endif
 }
 
 GLInstancingRenderer::~GLInstancingRenderer()
@@ -1512,6 +1522,11 @@ void GLInstancingRenderer::setLightPosition(const double lightPos[3])
 	m_data->m_lightPos[2] = lightPos[2];
 }
 
+void GLInstancingRenderer::setBackgroundColor(const double rgbBackground[3])
+{
+	glClearColor(rgbBackground[0], rgbBackground[1], rgbBackground[2], 1.f);
+}
+
 void GLInstancingRenderer::setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])
 {
 	for (int i = 0; i < 16; i++)

examples/OpenGLWindow/GLInstancingRenderer.h

@@ -127,7 +127,7 @@ public:
 	void setLightSpecularIntensity(const float lightSpecularIntensity[3]);
 	virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]);
 	virtual void setProjectiveTexture(bool useProjectiveTexture);
-
+	virtual void setBackgroundColor(const double rgbBackground[3]);
 	virtual void resize(int width, int height);
 	virtual int getScreenWidth()
 	{

examples/OpenGLWindow/SimpleOpenGL2Renderer.h

@@ -30,6 +30,7 @@ public:
 	virtual void setShadowMapIntensity(double shadowMapIntensity) {}
 	virtual void setShadowMapWorldSize(float worldSize) {}
 	virtual void resize(int width, int height);
+	virtual void setBackgroundColor(const double rgbBackground[3]) {}
 
 	virtual void removeAllInstances();
 	virtual void removeGraphicsInstance(int instanceUid);

examples/Planar2D/Planar2D.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Planar2D/Planar2D.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Raycast/RaytestDemo.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/Raycast/RaytestDemo.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/RigidBody/KinematicRigidBodyExample.cpp

@@ -0,0 +1,234 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "KinematicRigidBodyExample.h"
+//#define USE_MOTIONSTATE 1
+#include "btBulletDynamicsCommon.h"
+#include "LinearMath/btTransformUtil.h"
+#define ARRAY_SIZE_Y 5
+#define ARRAY_SIZE_X 5
+#define ARRAY_SIZE_Z 5
+
+#include "LinearMath/btVector3.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../OpenGLWindow/ShapeData.h"
+
+#include "../CommonInterfaces/CommonRigidBodyBase.h"
+
+
+void kinematicPreTickCallback(btDynamicsWorld* world, btScalar deltaTime)
+{
+	btRigidBody* groundBody = (btRigidBody*)world->getWorldUserInfo();
+	btTransform predictedTrans;
+	btVector3 linearVelocity(0, 0, 0);
+	btVector3 angularVelocity(0, 0.1, 0);
+	btTransformUtil::integrateTransform(groundBody->getWorldTransform(), linearVelocity, angularVelocity, deltaTime, predictedTrans);
+#ifdef USE_MOTIONSTATE
+	groundBody->getMotionState()->setWorldTransform(predictedTrans);
+#else
+	m_groundBody->setWorldTransform(predictedTrans);
+#endif
+}
+
+struct KinematicRigidBodyExample : public CommonRigidBodyBase
+{
+	btRigidBody* m_groundBody;
+
+	KinematicRigidBodyExample(struct GUIHelperInterface* helper)
+		: CommonRigidBodyBase(helper),
+		  m_groundBody(0)
+	{
+	}
+
+	virtual void stepSimulation(float deltaTime)
+	{
+		if (m_dynamicsWorld)
+		{
+			m_dynamicsWorld->stepSimulation(deltaTime);
+		}
+	}
+
+	virtual ~KinematicRigidBodyExample() {}
+	virtual void initPhysics();
+	virtual void renderScene();
+	void resetCamera()
+	{
+		float dist = 4;
+		float pitch = -30;
+		float yaw = 50;
+		float targetPos[3] = {0, 0, 0};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+void KinematicRigidBodyExample::initPhysics()
+{
+	m_guiHelper->setUpAxis(1);
+
+	createEmptyDynamicsWorld();
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+
+	if (m_dynamicsWorld->getDebugDrawer())
+		m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints);
+
+	///create a few basic rigid bodies
+	btScalar halfExtentsX = 10.0;
+	btScalar halfExtentsY = 0.1;
+	btScalar halfExtentsZ = 10.0;
+
+	btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10.), btScalar(0.1), btScalar(10.)));
+	btTransform groundTransform;
+	groundTransform.setIdentity();
+	groundTransform.setOrigin(btVector3(0, -halfExtentsY, 0));
+	m_collisionShapes.push_back(groundShape);
+
+
+
+	{
+		btScalar mass(0.);
+		//rigidbody is dynamic if and only if mass is non zero, otherwise static
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0, 0, 0);
+
+#ifdef USE_MOTIONSTATE
+		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
+		btRigidBody::btRigidBodyConstructionInfo cInfo(mass, myMotionState, groundShape, localInertia);
+		m_groundBody = new btRigidBody(cInfo);
+#else
+		m_groundBody = new btRigidBody(mass, 0, shape, localInertia);
+		m_groundBody->setWorldTransform(startTransform);
+#endif  //
+
+		m_groundBody->setUserIndex(-1);
+		
+		m_groundBody->forceActivationState(DISABLE_DEACTIVATION);
+		m_groundBody->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_STATIC_OBJECT);
+		m_dynamicsWorld->addRigidBody(m_groundBody);
+		
+	}
+	m_dynamicsWorld->setInternalTickCallback(kinematicPreTickCallback, m_groundBody, true);
+	{
+		int strideInBytes = 9 * sizeof(float);
+		int numVertices = sizeof(cube_vertices_textured) / strideInBytes;
+		int numIndices = sizeof(cube_indices) / sizeof(int);
+		btScalar textureScaling = 40.0;
+		btAlignedObjectArray<GfxVertexFormat1> verts;
+		verts.resize(numVertices);
+		for (int i = 0; i < numVertices; i++)
+		{
+			verts[i].x = halfExtentsX * cube_vertices_textured[i * 9];
+			verts[i].y = halfExtentsY * cube_vertices_textured[i * 9 + 1];
+			verts[i].z = halfExtentsZ * cube_vertices_textured[i * 9 + 2];
+			verts[i].w = cube_vertices_textured[i * 9 + 3];
+			verts[i].nx = cube_vertices_textured[i * 9 + 4];
+			verts[i].ny = cube_vertices_textured[i * 9 + 5];
+			verts[i].nz = cube_vertices_textured[i * 9 + 6];
+			verts[i].u = cube_vertices_textured[i * 9 + 7] * textureScaling;
+			verts[i].v = cube_vertices_textured[i * 9 + 8] * textureScaling;
+		}
+
+		int red = 173;
+		int green = 199;
+		int blue = 255;
+		btAlignedObjectArray<unsigned char> rgbaTexture;
+		int textureWidth = 256;
+		int textureHeight = 256;
+		rgbaTexture.resize(textureWidth * textureHeight * 3);
+
+		for (int i = 0; i < textureWidth * textureHeight * 3; i++)
+			rgbaTexture[i] = 255;
+		for (int i = 0; i < textureWidth; i++)
+		{
+			for (int j = 0; j < textureHeight; j++)
+			{
+				int a = i < textureWidth / 2 ? 1 : 0;
+				int b = j < textureWidth / 2 ? 1 : 0;
+
+				if (a == b)
+				{
+					rgbaTexture[(i + j * textureWidth) * 3 + 0] = red;
+					rgbaTexture[(i + j * textureWidth) * 3 + 1] = green;
+					rgbaTexture[(i + j * textureWidth) * 3 + 2] = blue;
+				}
+			}
+		}
+		bool flipPixelsY = false;
+		int textureIndex = m_guiHelper->getRenderInterface()->registerTexture(&rgbaTexture[0], textureWidth, textureHeight, flipPixelsY);
+		int shapeId = m_guiHelper->getRenderInterface()->registerShape(&verts[0].x, numVertices, cube_indices, numIndices, B3_GL_TRIANGLES, textureIndex);
+		btVector3 scaling(1, 1, 1);
+		btVector4 color(1, 1, 1, 1);
+		btQuaternion orn;
+		groundTransform.getBasis().getRotation(orn);
+		int graphicsInstanceId = m_guiHelper->getRenderInterface()->registerGraphicsInstance(shapeId, groundTransform.getOrigin(), orn, color, scaling);
+		groundShape->setUserIndex(shapeId);
+		m_groundBody->setUserIndex(graphicsInstanceId);
+	
+	}
+
+	if (1)
+	{
+		//create a few dynamic rigidbodies
+		// Re-using the same collision is better for memory usage and performance
+
+		btBoxShape* colShape = createBoxShape(btVector3(.1, .1, .1));
+
+		//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
+		m_collisionShapes.push_back(colShape);
+
+		/// Create Dynamic Objects
+		btTransform startTransform;
+		startTransform.setIdentity();
+
+		btScalar mass(1.f);
+
+		//rigidbody is dynamic if and only if mass is non zero, otherwise static
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0, 0, 0);
+		if (isDynamic)
+			colShape->calculateLocalInertia(mass, localInertia);
+
+		for (int k = 0; k < ARRAY_SIZE_Y; k++)
+		{
+			for (int i = 0; i < ARRAY_SIZE_X; i++)
+			{
+				for (int j = 0; j < ARRAY_SIZE_Z; j++)
+				{
+					startTransform.setOrigin(btVector3(
+						btScalar(0.2 * i),
+						btScalar(2 + .2 * k),
+						btScalar(0.2 * j)));
+
+					createRigidBody(mass, startTransform, colShape);
+				}
+			}
+		}
+	}
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+void KinematicRigidBodyExample::renderScene()
+{
+	CommonRigidBodyBase::renderScene();
+}
+
+CommonExampleInterface* KinematicRigidBodyExampleCreateFunc(CommonExampleOptions& options)
+{
+	return new KinematicRigidBodyExample(options.m_guiHelper);
+}
+
+B3_STANDALONE_EXAMPLE(KinematicRigidBodyExampleCreateFunc)

examples/RigidBody/KinematicRigidBodyExample.h

@@ -0,0 +1,21 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef KINEMATIC_RIGID_BODY_EXAMPLE_H
+#define KINEMATIC_RIGID_BODY_EXAMPLE_H
+
+class CommonExampleInterface* KinematicRigidBodyExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //KINEMATIC_RIGID_BODY_EXAMPLE_H

examples/RobotSimulator/CMakeLists.txt

@@ -58,7 +58,8 @@ ELSE(WIN32)
 		ADD_DEFINITIONS("-DGLEW_STATIC")
 		ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
 
-		LINK_LIBRARIES(  pthread ${DL} )
+		FIND_PACKAGE(Threads)
+		LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
 	ENDIF(APPLE)
 ENDIF(WIN32)
 
@@ -110,7 +111,8 @@ IF(WIN32)
 ELSE()
 	IF(APPLE)
 	ELSE(APPLE)
-		TARGET_LINK_LIBRARIES( App_RobotSimulator_NoGUI pthread ${DL} )
+		FIND_PACKAGE(Threads)
+		LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
 	ENDIF(APPLE)
 ENDIF(WIN32)
 
@@ -144,10 +146,10 @@ IF(WIN32)
                 TARGET_LINK_LIBRARIES(App_HelloBulletRobotics ws2_32 Winmm)
         ENDIF(BUILD_ENET OR BUILD_CLSOCKET)
 ELSE()
-        IF(APPLE)
-        ELSE(APPLE)
-                TARGET_LINK_LIBRARIES( App_HelloBulletRobotics pthread ${DL} )
-        ENDIF(APPLE)
+	IF(NOT APPLE)
+		FIND_PACKAGE(Threads)
+		TARGET_LINK_LIBRARIES( App_HelloBulletRobotics ${CMAKE_THREAD_LIBS_INIT} ${DL} )
+	ENDIF(NOT APPLE)
 ENDIF(WIN32)
 
 

examples/RollingFrictionDemo/RollingFrictionDemo.cpp

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/RollingFrictionDemo/RollingFrictionDemo.h

@@ -1,6 +1,6 @@
 /*
 Bullet Continuous Collision Detection and Physics Library
-Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
+Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
 
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.

examples/SharedMemory/CMakeLists.txt

@@ -120,8 +120,9 @@ IF (WIN32)
                 ${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc
         )
 ELSE(WIN32)
-			 IF(APPLE)
-			        LINK_LIBRARIES(  pthread dl )
+	FIND_PACKAGE(Threads)
+	LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
+	IF(APPLE)
 			        ADD_EXECUTABLE(App_PhysicsServer_SharedMemory
 			                ${SharedMemory_SRCS}
 			                ../MultiThreading/b3PosixThreadSupport.cpp
@@ -129,8 +130,7 @@ ELSE(WIN32)
 			                main.cpp
 			        )
 
-        ELSE(APPLE)
-                                LINK_LIBRARIES(  pthread ${DL} )
+	ELSE(APPLE)
 			        ADD_EXECUTABLE(App_PhysicsServer_SharedMemory
 			                ${SharedMemory_SRCS}
 			                ../MultiThreading/b3PosixThreadSupport.cpp
@@ -180,8 +180,9 @@ IF (WIN32)
 			          ${BULLET_PHYSICS_SOURCE_DIR}/build3/bullet.rc
         )
 ELSE(WIN32)
+	FIND_PACKAGE(Threads)
+	LINK_LIBRARIES( ${CMAKE_THREAD_LIBS_INIT} ${DL} )
 			 IF(APPLE)
-			        LINK_LIBRARIES(  pthread dl )
 			        FIND_LIBRARY(COCOA NAMES Cocoa)
               MESSAGE(${COCOA})
               LINK_LIBRARIES(${COCOA} ${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY})
@@ -197,7 +198,6 @@ ELSE(WIN32)
 			        )
 
         ELSE(APPLE)
-				LINK_LIBRARIES(  pthread ${DL} )
          			ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
               ADD_DEFINITIONS("-DGLEW_STATIC")
               ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
@@ -225,6 +225,8 @@ ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
 
 
 #VR/OpenVR on Windows and Mac OSX
+IF(USE_OPENVR)
+
 IF (WIN32 OR APPLE)
 
 INCLUDE_DIRECTORIES(
@@ -391,9 +393,10 @@ ELSE(WIN32 OR APPLE)
                -std=c++11
        )
 
+       FIND_PACKAGE(Threads)
        target_link_libraries(App_PhysicsServer_SharedMemory_VR PRIVATE
                openvr_api
-               pthread
+               ${CMAKE_THREAD_LIBS_INIT}
                ${DL}
                Bullet3Common
                BulletWorldImporter
@@ -413,3 +416,5 @@ ELSE(WIN32 OR APPLE)
        ENDIF(INTERNAL_ADD_POSTFIX_EXECUTABLE_NAMES)
 
 ENDIF(WIN32 OR APPLE)
+ENDIF(USE_OPENVR)
+

examples/SharedMemory/GraphicsServerExample.cpp

@@ -347,6 +347,11 @@ void TCPThreadFunc(void* userPtr, void* lsMemory)
 
 									case GFX_CMD_SET_VISUALIZER_FLAG:
 									{
+										//disable single step rendering for GraphicsServer
+										if (args->m_cmdPtr->m_visualizerFlagCommand.m_visualizerFlag == COV_ENABLE_SINGLE_STEP_RENDERING)
+										{
+											args->m_cmdPtr->m_visualizerFlagCommand.m_visualizerFlag = 0;
+										}
 										args->submitCommand();
 										while (args->isCommandOutstanding())
 										{
@@ -740,7 +745,8 @@ public:
 			}
 			case GFX_CMD_SET_VISUALIZER_FLAG:
 			{
-				if (clientCmd.m_visualizerFlagCommand.m_visualizerFlag != COV_ENABLE_RENDERING)
+				if ((clientCmd.m_visualizerFlagCommand.m_visualizerFlag != COV_ENABLE_RENDERING) &&
+					(clientCmd.m_visualizerFlagCommand.m_visualizerFlag != COV_ENABLE_SINGLE_STEP_RENDERING))
 				{
 					//printf("clientCmd.m_visualizerFlag.m_visualizerFlag: %d, clientCmd.m_visualizerFlag.m_enable %d\n",
 					//	clientCmd.m_visualizerFlagCommand.m_visualizerFlag, clientCmd.m_visualizerFlagCommand.m_enable);

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -260,6 +260,17 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
 	}
 }
 
+B3_SHARED_API void b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_LOAD_MJCF);
+	if (command->m_type == CMD_LOAD_MJCF)
+	{
+		command->m_updateFlags |= URDF_ARGS_USE_MULTIBODY;
+		command->m_mjcfArguments.m_useMultiBody = useMultiBody;
+	}
+}
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -977,6 +988,19 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand2(b3SharedM
 	return (b3SharedMemoryCommandHandle)command;
 }
 
+
+B3_SHARED_API b3SharedMemoryCommandHandle b3InitPerformCollisionDetectionCommand(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+	command->m_type = CMD_PERFORM_COLLISION_DETECTION;
+	command->m_updateFlags = 0;
+	return (b3SharedMemoryCommandHandle)command;
+}
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -1043,6 +1067,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2Internal(b3S
 		command->m_sendDesiredStateCommandArgument.m_Kp[i] = 0;
 		command->m_sendDesiredStateCommandArgument.m_Kd[i] = 0;
 		command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 0;
+		command->m_sendDesiredStateCommandArgument.m_damping[i] = 0;
 	}
 	command->m_sendDesiredStateCommandArgument.m_desiredStateQ[3] = 1;
 	return (b3SharedMemoryCommandHandle)command;
@@ -1090,6 +1115,22 @@ B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle,
 	return 0;
 }
 
+B3_SHARED_API int b3JointControlSetKpMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kps, int dofCount)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
+	{
+		for (int dof = 0; dof < dofCount; dof++)
+		{
+			command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex + dof] = kps[dof];
+			command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
+			command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_KP;
+		}
+	}
+	return 0;
+}
+
 B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -1103,6 +1144,22 @@ B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle,
 	return 0;
 }
 
+B3_SHARED_API int b3JointControlSetKdMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kds, int dofCount)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
+	{
+		for (int dof = 0; dof < dofCount; dof++)
+		{
+			command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex + dof] = kds[dof];
+			command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
+			command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_KD;
+		}
+	}
+	return 0;
+}
+
 B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -1161,6 +1218,35 @@ B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryComm
 	return 0;
 }
 
+B3_SHARED_API int b3JointControlSetDamping(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
+	{
+		command->m_sendDesiredStateCommandArgument.m_damping[dofIndex] = value;
+		command->m_updateFlags |= SIM_DESIRED_STATE_HAS_DAMPING;
+		command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_DAMPING;
+	}
+	return 0;
+}
+
+B3_SHARED_API int b3JointControlSetDampingMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* damping, int dofCount)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
+	{
+		for (int dof = 0; dof < dofCount; dof++)
+		{
+			command->m_sendDesiredStateCommandArgument.m_damping[dofIndex+dof] = damping[dof];
+			command->m_updateFlags |= SIM_DESIRED_STATE_HAS_DAMPING;
+			command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_DAMPING;
+		}
+	}
+	return 0;
+}
+
 B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -1398,6 +1484,28 @@ B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle co
 	return -1;
 }
 
+B3_SHARED_API b3SharedMemoryCommandHandle b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int numVertices, const double* vertices)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	if (cl)
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_RESET_MESH_DATA;
+		command->m_updateFlags = 0;
+		command->m_resetMeshDataArgs.m_numVertices = numVertices;
+		command->m_resetMeshDataArgs.m_bodyUniqueId = bodyUniqueId;
+		command->m_resetMeshDataArgs.m_flags = 0;
+		int totalUploadSizeInBytes = numVertices * sizeof(double) *3;
+		cl->uploadBulletFileToSharedMemory((const char*)vertices, totalUploadSizeInBytes);
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3GetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -2097,6 +2205,7 @@ B3_SHARED_API int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandl
 			command->m_createMultiBodyArgs.m_linkJointAxis[3 * linkIndex + 2] = linkJointAxis[2];
 
 			command->m_createMultiBodyArgs.m_linkMasses[linkIndex] = linkMass;
+			
 			command->m_createMultiBodyArgs.m_numLinks++;
 			return numLinks;
 		}
@@ -3219,6 +3328,29 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHa
 	return 0;
 }
 
+B3_SHARED_API int b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double sleepThreshold)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
+	command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_changeDynamicsInfoArgs.m_sleepThreshold = sleepThreshold;
+	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SLEEP_THRESHOLD;
+	return 0;
+}
+
+
+
+B3_SHARED_API int b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int dynamicType)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
+	b3Assert(dynamicType == eDynamic || dynamicType == eStatic || dynamicType == eKinematic);
+	command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
+	command->m_changeDynamicsInfoArgs.m_dynamicType = dynamicType;
+	command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_DYNAMIC_TYPE;
+	return 0;
+}
 
 
 
@@ -3865,6 +3997,19 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsCli
 	return (b3SharedMemoryCommandHandle)command;
 }
 
+B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestBodyInfoCommand(b3PhysicsClientHandle physClient, int bodyUniqueId)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+	b3Assert(command);
+
+	command->m_type = CMD_REQUEST_BODY_INFO;
+	command->m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
+	return (b3SharedMemoryCommandHandle)command;
+}
+
 B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient)
 {
 	PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -4461,7 +4606,7 @@ B3_SHARED_API void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetP
 
 	b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547);      // rads per deg
 	b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547);  // rads per deg
-	b3Scalar rollRad = 0.0;
+	b3Scalar rollRad = roll * b3Scalar(0.01745329251994329547);      // rads per deg
 	b3Quaternion eyeRot;
 
 	int forwardAxis(-1);
@@ -5953,6 +6098,21 @@ B3_SHARED_API void b3ConfigureOpenGLVisualizerSetLightPosition(b3SharedMemoryCom
 	}
 }
 
+B3_SHARED_API void b3ConfigureOpenGLVisualizerSetLightRgbBackground(b3SharedMemoryCommandHandle commandHandle, const float rgbBackground[3])
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command);
+	b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
+	if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
+	{
+		command->m_updateFlags |= COV_SET_RGB_BACKGROUND;
+		command->m_configureOpenGLVisualizerArguments.m_rgbBackground[0] = rgbBackground[0];
+		command->m_configureOpenGLVisualizerArguments.m_rgbBackground[1] = rgbBackground[1];
+		command->m_configureOpenGLVisualizerArguments.m_rgbBackground[2] = rgbBackground[2];
+	}
+}
+
+
 B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapResolution(b3SharedMemoryCommandHandle commandHandle, int shadowMapResolution)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -106,6 +106,9 @@ extern "C"
 	///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
 
+	// Sync the body info of a single body. Useful when a new body has been added by a different client (e,g, when detecting through a body added notification).
+	B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestBodyInfoCommand(b3PhysicsClientHandle physClient, int bodyUniqueId);
+
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId);
 
 	///return the total number of bodies in the simulation
@@ -155,8 +158,9 @@ extern "C"
 	B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double anisotropicFriction[]);
 	B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimit(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLowerLimit, double jointUpperLimit);
 	B3_SHARED_API int b3ChangeDynamicsInfoSetJointLimitForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double jointLimitForce);
+	B3_SHARED_API int b3ChangeDynamicsInfoSetDynamicType(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, int dynamicType);
 
-	
+	B3_SHARED_API int b3ChangeDynamicsInfoSetSleepThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double sleepThreshold);
 	
 	B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
 	B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
@@ -217,7 +221,9 @@ extern "C"
 	B3_SHARED_API void b3ConfigureOpenGLVisualizerSetLightPosition(b3SharedMemoryCommandHandle commandHandle, const float lightPosition[3]);
 	B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapResolution(b3SharedMemoryCommandHandle commandHandle, int shadowMapResolution);
 	B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapIntensity(b3SharedMemoryCommandHandle commandHandle, double shadowMapIntensity);
+	B3_SHARED_API void b3ConfigureOpenGLVisualizerSetLightRgbBackground(b3SharedMemoryCommandHandle commandHandle, const float rgbBackground[3]);
 	
+
 	B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(b3SharedMemoryCommandHandle commandHandle, int shadowMapWorldSize);
 	B3_SHARED_API void b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(b3SharedMemoryCommandHandle commandHandle, double remoteSyncTransformInterval);
 	
@@ -382,6 +388,8 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
 
+	B3_SHARED_API b3SharedMemoryCommandHandle b3InitPerformCollisionDetectionCommand(b3PhysicsClientHandle physClient);
+
 	B3_SHARED_API int b3GetStatusForwardDynamicsAnalyticsData(b3SharedMemoryStatusHandle statusHandle, struct b3ForwardDynamicsAnalyticsArgs* analyticsData);
 
 
@@ -413,6 +421,8 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit2(b3SharedMemoryCommandHandle commandHandle, const char* fileName);
 	B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
+	B3_SHARED_API void b3LoadMJCFCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
+	
 
 	///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
 	B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
@@ -482,7 +492,9 @@ extern "C"
 	B3_SHARED_API int b3JointControlSetDesiredPositionMultiDof(b3SharedMemoryCommandHandle commandHandle, int qIndex, const double* position, int dofCount);
 
 	B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+	B3_SHARED_API int b3JointControlSetKpMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kps, int dofCount);
 	B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+	B3_SHARED_API int b3JointControlSetKdMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* kds, int dofCount);
 	B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity);
 
 	///Only use when controlMode is CONTROL_MODE_VELOCITY
@@ -492,6 +504,8 @@ extern "C"
 
 	B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
 	B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount);
+	B3_SHARED_API int b3JointControlSetDamping(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+	B3_SHARED_API int b3JointControlSetDampingMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* damping, int dofCount);
 	
 	///Only use if when controlMode is CONTROL_MODE_TORQUE,
 	B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
@@ -523,6 +537,7 @@ extern "C"
 	
 	B3_SHARED_API void b3GetMeshData(b3PhysicsClientHandle physClient, struct b3MeshData* meshData);
 
+	B3_SHARED_API b3SharedMemoryCommandHandle b3ResetMeshDataCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int num_vertices, const double* vertices);
 
 	B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
 	B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle, double radius);

examples/SharedMemory/PhysicsClientExample.cpp

@@ -13,6 +13,12 @@
 #include "PhysicsDirectC_API.h"
 #include "PhysicsClientC_API.h"
 #include "PhysicsServerSharedMemory.h"
+#include <stdio.h>
+#ifdef _WIN32
+#define safe_printf _snprintf
+#else
+#define safe_printf snprintf
+#endif
 struct MyMotorInfo2
 {
 	btScalar m_velTarget;
@@ -655,7 +661,7 @@ void PhysicsClientExample::createButtons()
 					if (m_numMotors < MAX_NUM_MOTORS)
 					{
 						char motorName[1026];
-						snprintf(motorName, sizeof(motorName), "%s q", info.m_jointName);
+						safe_printf(motorName, sizeof(motorName), "%s q", info.m_jointName);
 						// MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
 						MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors];
 						motorInfo->m_velTarget = 0.f;

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -702,6 +702,16 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
 				}
 				break;
 			}
+			case CMD_PERFORM_COLLISION_DETECTION_COMPLETED:
+			{
+				B3_PROFILE("CMD_PERFORM_COLLISION_DETECTION_COMPLETED");
+
+				if (m_data->m_verboseOutput)
+				{
+					b3Printf("Server completed performing collision detection");
+				}
+				break;
+			}
 			case CMD_URDF_LOADING_FAILED:
 			{
 				B3_PROFILE("CMD_URDF_LOADING_FAILED");
@@ -1511,12 +1521,18 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
 				b3Warning("Removing user data failed");
 				break;
 			}
+			case CMD_RESET_MESH_DATA_FAILED:
+			{
+				b3Warning("resetMeshData failed");
+				break;
+			}
 			case CMD_REQUEST_USER_DATA_COMPLETED:
 			case CMD_SYNC_USER_DATA_COMPLETED:
 			case CMD_REMOVE_USER_DATA_COMPLETED:
 			case CMD_ADD_USER_DATA_COMPLETED:
 			case CMD_REMOVE_STATE_FAILED:
 			case CMD_REMOVE_STATE_COMPLETED:
+			case CMD_RESET_MESH_DATA_COMPLETED:
 			{
 				break;
 			}

examples/SharedMemory/PhysicsDirect.cpp

@@ -1008,21 +1008,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 
 				m_data->m_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
 				m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
-
-				bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
-
-				b3Clock clock;
-				double startTime = clock.getTimeInSeconds();
-				double timeOutInSeconds = m_data->m_timeOutInSeconds;
-				while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
-				{
-					hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
-				}
-
-				if (hasStatus)
-				{
-					processBodyJointInfo(bodyUniqueId, m_data->m_tmpInfoStatus);
-				}
+				processRequestBodyInfo(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus);
 			}
 			break;
 		}
@@ -1166,6 +1152,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		{
 			break;
 		}
+		case CMD_PERFORM_COLLISION_DETECTION_COMPLETED:
+		{
+			break;
+		}
 		case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED:
 		{
 			break;
@@ -1302,6 +1292,14 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 			}
 			break;
 		}
+		case CMD_RESET_MESH_DATA_COMPLETED:
+		{
+			break;
+		}
+		case CMD_RESET_MESH_DATA_FAILED:
+		{
+			break;
+		}
 		case CMD_REMOVE_STATE_FAILED:
 		{
 			break;
@@ -1393,6 +1391,22 @@ bool PhysicsDirect::processCustomCommand(const struct SharedMemoryCommand& orgCo
 	return m_data->m_hasStatus;
 }
 
+bool PhysicsDirect::processRequestBodyInfo(const struct SharedMemoryCommand& command, SharedMemoryStatus& status) {
+	bool hasStatus = m_data->m_commandProcessor->processCommand(command, status, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+	b3Clock clock;
+	double startTime = clock.getTimeInSeconds();
+	double timeOutInSeconds = m_data->m_timeOutInSeconds;
+	while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
+	{
+		hasStatus = m_data->m_commandProcessor->receiveStatus(status, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+	}
+	if (hasStatus) {
+		processBodyJointInfo(command.m_sdfRequestInfoArgs.m_bodyUniqueId, status);
+	}
+	m_data->m_hasStatus = hasStatus;
+	return m_data->m_hasStatus;
+}
+
 bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
 {
 	if (command.m_type == CMD_CUSTOM_COMMAND)
@@ -1422,10 +1436,14 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
 		return processOverlappingObjects(command);
 	}
 
-  if (command.m_type == CMD_REQUEST_MESH_DATA)
+	if (command.m_type == CMD_REQUEST_MESH_DATA)
 	{
 		return processMeshData(command);
 	}
+	if (command.m_type == CMD_REQUEST_BODY_INFO)
+	{
+		return processRequestBodyInfo(command, m_data->m_serverStatus);
+	}
 
 	bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
 	m_data->m_hasStatus = hasStatus;

examples/SharedMemory/PhysicsDirect.h

@@ -28,6 +28,8 @@ protected:
 
 	void processAddUserData(const struct SharedMemoryStatus& serverCmd);
 
+	bool processRequestBodyInfo(const struct SharedMemoryCommand& command, SharedMemoryStatus& status);
+
 	bool processCustomCommand(const struct SharedMemoryCommand& orgCommand);
 
 	void postProcessStatus(const struct SharedMemoryStatus& serverCmd);

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -32,6 +32,7 @@ protected:
 	bool processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processCreateVisualShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processResetMeshDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processSetVRCameraStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -44,6 +45,7 @@ protected:
 	bool processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool processRequestDeformableContactpointHelper(const struct SharedMemoryCommand& clientCmd);
 	bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -55,6 +57,7 @@ protected:
 	bool processProfileTimingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
+	bool performCollisionDetectionCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
 	bool processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);

examples/SharedMemory/PhysicsServerExample.cpp

@@ -133,6 +133,7 @@ enum MultiThreadedGUIHelperCommunicationEnums
 	eGUIUserDebugRemoveAllParameters,
 	eGUIHelperResetCamera,
 	eGUIHelperChangeGraphicsInstanceFlags,
+	eGUIHelperSetRgbBackground,
 };
 
 #include <stdio.h>
@@ -709,7 +710,7 @@ public:
 	{
 		BT_PROFILE("mainThreadRelease");
 
-		getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
+		setSharedParam(1, eGUIHelperIdle);
 		getCriticalSection3()->lock();
 		getCriticalSection2()->unlock();
 		getCriticalSection()->lock();
@@ -724,7 +725,9 @@ public:
 
 		if (m_skipGraphicsUpdate)
 		{
+			this->m_csGUI->lock();
 			getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
+			this->m_csGUI->unlock();
 			m_cs->unlock();
 			return;
 		}
@@ -734,9 +737,18 @@ public:
 		m_cs3->lock();
 		m_cs3->unlock();
 
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
+
+		m_csGUI->lock();
+		unsigned int cachedSharedParam = m_cs->getSharedParam(1);
+		m_csGUI->unlock();
+
+		
+		while (cachedSharedParam != eGUIHelperIdle)
 		{
 			b3Clock::usleep(0);
+			m_csGUI->lock();
+			cachedSharedParam = m_cs->getSharedParam(1);
+			m_csGUI->unlock();
 		}
 	}
 
@@ -818,10 +830,11 @@ public:
 	btVector3 m_color3;
 	virtual void createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color)
 	{
-		m_body = body;
-		m_color3 = color;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperCreateRigidBodyGraphicsObject);
+
+    m_body = body;
+		m_color3 = color;
+		setSharedParam(1, eGUIHelperCreateRigidBodyGraphicsObject);
 		workerThreadWait();
 	}
 
@@ -830,19 +843,21 @@ public:
 
 	virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj, const btVector3& color)
 	{
-		m_obj = obj;
-		m_color2 = color;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperCreateCollisionObjectGraphicsObject);
+
+    m_obj = obj;
+		m_color2 = color;
+		setSharedParam(1, eGUIHelperCreateCollisionObjectGraphicsObject);
 		workerThreadWait();
 	}
 
 	btCollisionShape* m_colShape;
 	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
 	{
-		m_colShape = collisionShape;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperCreateCollisionShapeGraphicsObject);
+
+    m_colShape = collisionShape;
+    setSharedParam(1, eGUIHelperCreateCollisionShapeGraphicsObject);
 		workerThreadWait();
 	}
 
@@ -889,9 +904,10 @@ public:
 
 	virtual void removeTexture(int textureUid)
 	{
-		m_removeTextureUid = textureUid;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperRemoveTexture);
+
+    m_removeTextureUid = textureUid;
+		setSharedParam(1, eGUIHelperRemoveTexture);
 
 		workerThreadWait();
 	}
@@ -901,11 +917,12 @@ public:
 	int m_updateNumShapeVertices;
 	virtual void updateShape(int shapeIndex, float* vertices, int numVertices)
 	{
-		m_updateShapeIndex = shapeIndex;
+		m_cs->lock();
+
+    m_updateShapeIndex = shapeIndex;
 		m_updateShapeVertices = vertices;
 		m_updateNumShapeVertices = numVertices;
-		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperUpdateShape);
+		setSharedParam(1, eGUIHelperUpdateShape);
 		workerThreadWait();
 	}
 	virtual int registerTexture(const unsigned char* texels, int width, int height)
@@ -915,12 +932,13 @@ public:
 		{
 			return *cachedTexture;
 		}
-		m_texels = texels;
+		m_cs->lock();
+
+    m_texels = texels;
 		m_textureWidth = width;
 		m_textureHeight = height;
 
-		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperRegisterTexture);
+		setSharedParam(1, eGUIHelperRegisterTexture);
 
 		workerThreadWait();
 		m_cachedTextureIds.insert(texels, m_textureId);
@@ -928,31 +946,44 @@ public:
 	}
 	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType, int textureId)
 	{
-		m_vertices = vertices;
+		m_cs->lock();
+    m_csGUI->lock();
+    m_vertices = vertices;
 		m_numvertices = numvertices;
 		m_indices = indices;
 		m_numIndices = numIndices;
 		m_primitiveType = primitiveType;
 		m_textureId = textureId;
-
-		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperRegisterGraphicsShape);
+    m_csGUI->unlock();
+		setSharedParam(1, eGUIHelperRegisterGraphicsShape);
 		workerThreadWait();
 
-		return m_shapeIndex;
+    m_csGUI->lock();
+    int shapeIndex = m_shapeIndex;
+    m_csGUI->unlock();
+
+
+		return shapeIndex;
 	}
 
 	int m_visualizerFlag;
 	int m_visualizerEnable;
 	int m_renderedFrames;
 
+  void setSharedParam(int slot, int param)
+  {
+    m_csGUI->lock();
+    m_cs->setSharedParam(slot, param);
+    m_csGUI->unlock();
+  }
 	void setVisualizerFlag(int flag, int enable)
 	{
-		m_visualizerFlag = flag;
+		m_cs->lock();
+
+    m_visualizerFlag = flag;
 		m_visualizerEnable = enable;
 
-		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperSetVisualizerFlag);
+		setSharedParam(1, eGUIHelperSetVisualizerFlag);
 		workerThreadWait();
 	}
 
@@ -970,16 +1001,16 @@ public:
 		m_scaling = scaling;
 
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperRegisterGraphicsInstance);
+		setSharedParam(1, eGUIHelperRegisterGraphicsInstance);
 		workerThreadWait();
 		return m_instanceId;
 	}
 
 	virtual void removeAllGraphicsInstances()
 	{
+    m_cs->lock();
 		m_cachedTextureIds.clear();
-		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperRemoveAllGraphicsInstances);
+    setSharedParam(1, eGUIHelperRemoveAllGraphicsInstances);
 		workerThreadWait();
 	}
 
@@ -988,7 +1019,7 @@ public:
 	{
 		m_graphicsInstanceRemove = graphicsUid;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperRemoveGraphicsInstance);
+		setSharedParam(1, eGUIHelperRemoveGraphicsInstance);
 		workerThreadWait();
 	}
 
@@ -999,7 +1030,7 @@ public:
 	{
 		m_getShapeIndex_instance = instance;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperGetShapeIndexFromInstance);
+		setSharedParam(1, eGUIHelperGetShapeIndexFromInstance);
 		getShapeIndex_shapeIndex = -1;
 		workerThreadWait();
 		return getShapeIndex_shapeIndex;
@@ -1012,7 +1043,7 @@ public:
 		m_graphicsInstanceChangeTextureShapeIndex = shapeIndex;
 		m_graphicsInstanceChangeTextureId = textureUid;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperChangeGraphicsInstanceTextureId);
+		setSharedParam(1, eGUIHelperChangeGraphicsInstanceTextureId);
 		workerThreadWait();
 	}
 
@@ -1028,7 +1059,7 @@ public:
 		m_changeTextureWidth = width;
 		m_changeTextureHeight = height;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperChangeTexture);
+	  setSharedParam(1, eGUIHelperChangeTexture);
 		workerThreadWait();
 	}
 
@@ -1042,7 +1073,7 @@ public:
 		m_rgbaColor[2] = rgbaColor[2];
 		m_rgbaColor[3] = rgbaColor[3];
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperChangeGraphicsInstanceRGBAColor);
+		setSharedParam(1, eGUIHelperChangeGraphicsInstanceRGBAColor);
 		workerThreadWait();
 	}
 
@@ -1054,11 +1085,22 @@ public:
 		m_graphicsInstanceFlagsInstanceUid = instanceUid;
 		m_graphicsInstanceFlags = flags;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperChangeGraphicsInstanceFlags);
+		setSharedParam(1, eGUIHelperChangeGraphicsInstanceFlags);
 		workerThreadWait();
 	}
 
-	
+	double m_rgbBackground[3];
+	virtual void setBackgroundColor(const double rgbBackground[3])
+	{
+		m_cs->lock();
+		m_rgbBackground[0] = rgbBackground[0];
+		m_rgbBackground[1] = rgbBackground[1];
+		m_rgbBackground[2] = rgbBackground[2];
+		
+		setSharedParam(1, eGUIHelperSetRgbBackground);
+		workerThreadWait();
+		
+	}
 
 
 	int m_graphicsInstanceChangeScaling;
@@ -1070,7 +1112,7 @@ public:
 		m_baseScaling[1] = scaling[1];
 		m_baseScaling[2] = scaling[2];
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperChangeGraphicsInstanceScaling);
+		setSharedParam(1, eGUIHelperChangeGraphicsInstanceScaling);
 		workerThreadWait();
 	}
 
@@ -1083,7 +1125,7 @@ public:
 		m_specularColor[1] = specularColor[1];
 		m_specularColor[2] = specularColor[2];
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperChangeGraphicsInstanceSpecularColor);
+		setSharedParam(1, eGUIHelperChangeGraphicsInstanceSpecularColor);
 		workerThreadWait();
 	}
 
@@ -1134,7 +1176,7 @@ public:
 #ifdef SYNC_CAMERA_USING_GUI_CS
 		m_csGUI->unlock();
 #else
-		m_cs->setSharedParam(1, eGUIHelperResetCamera);
+		setSharedParam(1, eGUIHelperResetCamera);
 		workerThreadWait();
 		m_childGuiHelper->resetCamera(camDist, yaw, pitch, camPosX, camPosY, camPosZ);
 #endif //SYNC_CAMERA_USING_GUI_CS
@@ -1182,7 +1224,7 @@ public:
 		m_destinationHeight = destinationHeight;
 		m_numPixelsCopied = numPixelsCopied;
 
-		m_cs->setSharedParam(1, eGUIHelperCopyCameraImageData);
+		setSharedParam(1, eGUIHelperCopyCameraImageData);
 		workerThreadWait();
 	}
 
@@ -1209,7 +1251,7 @@ public:
 		m_destinationHeight = destinationHeight;
 		m_numPixelsCopied = numPixelsCopied;
 
-		m_cs->setSharedParam(1, eGUIHelperDisplayCameraImageData);
+		setSharedParam(1, eGUIHelperDisplayCameraImageData);
 		workerThreadWait();
 	}
 
@@ -1235,7 +1277,7 @@ public:
 	{
 		m_dynamicsWorld = rbWorld;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIHelperAutogenerateGraphicsObjects);
+		setSharedParam(1, eGUIHelperAutogenerateGraphicsObjects);
 		workerThreadWait();
 	}
 
@@ -1288,7 +1330,7 @@ public:
 		m_tmpText.m_textOrientation[3] = orientation[3];
 
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIUserDebugAddText);
+		setSharedParam(1, eGUIUserDebugAddText);
 		m_resultUserDebugTextUid = -1;
 		workerThreadWait();
 
@@ -1321,7 +1363,7 @@ public:
 		m_tmpParam.m_itemUniqueId = m_uidGenerator++;
 
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIUserDebugAddParameter);
+	  setSharedParam(1, eGUIUserDebugAddParameter);
 		m_userDebugParamUid = -1;
 		workerThreadWait();
 
@@ -1376,7 +1418,7 @@ public:
 		{
 
 			m_cs->lock();
-			m_cs->setSharedParam(1, eGUIUserDebugAddLine);
+			setSharedParam(1, eGUIUserDebugAddLine);
 			m_resultDebugLineUid = -1;
 			workerThreadWait();
 		}
@@ -1389,20 +1431,20 @@ public:
 	{
 		m_removeDebugItemUid = debugItemUniqueId;
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIUserDebugRemoveItem);
+		setSharedParam(1, eGUIUserDebugRemoveItem);
 		workerThreadWait();
 	}
 	virtual void removeAllUserDebugItems()
 	{
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems);
+		setSharedParam(1, eGUIUserDebugRemoveAllItems);
 		workerThreadWait();
 	}
 
 	virtual void removeAllUserParameters()
 	{
 		m_cs->lock();
-		m_cs->setSharedParam(1, eGUIUserDebugRemoveAllParameters);
+		setSharedParam(1, eGUIUserDebugRemoveAllParameters);
 		workerThreadWait();
 	}
 
@@ -1414,7 +1456,7 @@ public:
 	{
 		m_cs->lock();
 		m_mp4FileName = mp4FileName;
-		m_cs->setSharedParam(1, eGUIDumpFramesToVideo);
+		setSharedParam(1, eGUIDumpFramesToVideo);
 		workerThreadWait();
 		m_mp4FileName = 0;
 	}
@@ -1887,8 +1929,10 @@ void PhysicsServerExample::initPhysics()
 		}
 	}
 	m_args[0].m_cs->lock();
+  m_args[0].m_csGUI->lock();
 	m_args[0].m_cs->setSharedParam(1, eGUIHelperIdle);
-	m_args[0].m_cs->unlock();
+  m_args[0].m_csGUI->unlock();
+  m_args[0].m_cs->unlock();
 	m_args[0].m_cs2->lock();
 
 	{
@@ -1995,7 +2039,12 @@ void PhysicsServerExample::updateGraphics()
 	}
 	m_multiThreadedHelper->getCriticalSectionGUI()->unlock();
 #endif
-	switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
+
+	m_multiThreadedHelper->getCriticalSectionGUI()->lock();
+	unsigned int cachedSharedParam = m_multiThreadedHelper->getCriticalSection()->getSharedParam(1);
+	m_multiThreadedHelper->getCriticalSectionGUI()->unlock();
+
+	switch (cachedSharedParam)
 	{
 		case eGUIHelperCreateCollisionShapeGraphicsObject:
 		{
@@ -2048,14 +2097,22 @@ void PhysicsServerExample::updateGraphics()
 		case eGUIHelperRegisterGraphicsShape:
 		{
 			B3_PROFILE("eGUIHelperRegisterGraphicsShape");
-			m_multiThreadedHelper->m_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
+			int shapeIndex = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
 				m_multiThreadedHelper->m_vertices,
 				m_multiThreadedHelper->m_numvertices,
 				m_multiThreadedHelper->m_indices,
 				m_multiThreadedHelper->m_numIndices,
 				m_multiThreadedHelper->m_primitiveType,
 				m_multiThreadedHelper->m_textureId);
-			m_multiThreadedHelper->mainThreadRelease();
+
+       m_multiThreadedHelper->getCriticalSectionGUI()->lock();
+       m_multiThreadedHelper->m_shapeIndex = shapeIndex;
+       m_multiThreadedHelper->getCriticalSectionGUI()->unlock();
+
+
+      m_multiThreadedHelper->mainThreadRelease();
+
+
 			break;
 		}
 
@@ -2156,8 +2213,8 @@ void PhysicsServerExample::updateGraphics()
 
 			m_multiThreadedHelper->m_childGuiHelper->setVisualizerFlag(m_multiThreadedHelper->m_visualizerFlag, m_multiThreadedHelper->m_visualizerEnable);
 
-			//postpone the release until an actual frame is rendered
-			if (flag == COV_ENABLE_SINGLE_STEP_RENDERING)
+			//postpone the release until an actual frame is rendered, unless it is a remote visualizer
+			if ((!m_multiThreadedHelper->m_childGuiHelper->isRemoteVisualizer()) && flag == COV_ENABLE_SINGLE_STEP_RENDERING)
 			{
 				m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperSetVisualizerFlagCheckRenderedFrame);
 			}
@@ -2267,6 +2324,13 @@ void PhysicsServerExample::updateGraphics()
 			break;
 		}
 
+		case eGUIHelperSetRgbBackground:
+		{
+			m_multiThreadedHelper->m_childGuiHelper->setBackgroundColor(m_multiThreadedHelper->m_rgbBackground);
+			m_multiThreadedHelper->mainThreadRelease();
+			break;
+		}
+
 		case eGUIHelperChangeGraphicsInstanceScaling:
 		{
 			B3_PROFILE("eGUIHelperChangeGraphicsInstanceScaling");

examples/SharedMemory/RemoteGUIHelperTCP.h

@@ -70,6 +70,7 @@ struct RemoteGUIHelperTCP : public GUIHelperInterface
 	virtual void removeAllUserDebugItems();
 
 	int uploadData(const unsigned char* data, int sizeInBytes, int slot);
+	virtual bool isRemoteVisualizer() { return true; }
 };
 
 #endif  //REMOTE_HELPER_TCP_H