Split Bullet/src/LinearMath/btSerializer.cpp into btSerializer64.cpp …

…to make it easier to rebuild serialization structure.

Add several MSVC optimization flags to cmake.
Bump up VERSION because serialization format changed
Expose btScalar& jointMaxForce, btScalar& jointMaxVelocity to 'getJointInfo2' API, add backwards compatibility to examples\Importers\ImportURDFDemo\URDFImporterInterface::getJointInfo.

pybullet: expose 4 more fields to getJointInfo: jointLowerLimit/jointUpperLimit/jointMaxForce/jointMaxVelocity
fix performance issue in CMD_ACTUAL_STATE_UPDATE_COMPLETED

CMakeLists.txt

@@ -107,6 +107,38 @@ IF(MSVC)
 		  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /fp:fast")
   ENDIF()
 
+	OPTION(USE_MSVC_STRING_POOLING "Use MSVC /GF string pooling option"	ON)
+	IF (USE_MSVC_STRING_POOLING)
+		SET(CMAKE_C_FLAGS "/GF ${CMAKE_C_FLAGS}")
+		SET(CMAKE_CXX_FLAGS "/GF ${CMAKE_CXX_FLAGS}")
+	ENDIF()
+
+	OPTION(USE_MSVC_FUNCTION_LEVEL_LINKING "Use MSVC /Gy function level linking option"	ON)
+	IF(USE_MSVC_FUNCTION_LEVEL_LINKING)
+		SET(CMAKE_C_FLAGS "/Gy ${CMAKE_C_FLAGS}")
+		SET(CMAKE_CXX_FLAGS "/Gy ${CMAKE_CXX_FLAGS}")
+		set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /OPT:REF")
+		set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /OPT:REF")
+	ENDIF(USE_MSVC_FUNCTION_LEVEL_LINKING)
+
+	OPTION(USE_MSVC_EXEPTIONS "Use MSVC C++ exceptions option"	OFF)
+
+
+
+	OPTION(USE_MSVC_COMDAT_FOLDING "Use MSVC /OPT:ICF COMDAT folding option"	ON)
+
+	IF(USE_MSVC_COMDAT_FOLDING)
+		set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /OPT:ICF")
+		set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} /OPT:ICF")
+	ENDIF()
+
+	OPTION(USE_MSVC_DISABLE_RTTI "Use MSVC /GR- disabled RTTI flags option"	ON)
+	IF(USE_MSVC_DISABLE_RTTI)
+	  STRING(REGEX REPLACE "/GR" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS}) # Disable RTTI
+		SET(CMAKE_C_FLAGS "/GR- ${CMAKE_C_FLAGS}")
+		SET(CMAKE_CXX_FLAGS "/GR- ${CMAKE_CXX_FLAGS}")
+	ENDIF(USE_MSVC_DISABLE_RTTI)
+
 	SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /wd4244 /wd4267")
 ENDIF(MSVC)
 

Extras/Serialize/BulletFileLoader/autogenerated/bullet.h

@@ -1368,6 +1368,10 @@ typedef struct bInvalidHandle {
         double m_jointTorque[6];
         double m_jointDamping;
         double m_jointFriction;
+        double m_jointLowerLimit;
+        double m_jointUpperLimit;
+        double m_jointMaxForce;
+        double m_jointMaxVelocity;
         char *m_linkName;
         char *m_jointName;
         btCollisionObjectDoubleData *m_linkCollider;
@@ -1395,6 +1399,10 @@ typedef struct bInvalidHandle {
         int m_posVarCount;
         float m_jointDamping;
         float m_jointFriction;
+        float m_jointLowerLimit;
+        float m_jointUpperLimit;
+        float m_jointMaxForce;
+        float m_jointMaxVelocity;
         char *m_linkName;
         char *m_jointName;
         btCollisionObjectFloatData *m_linkCollider;

Extras/Serialize/HeaderGenerator/apiGen.cpp

@@ -20,12 +20,13 @@
 #include "bDNA.h"
 #include "bBlenderFile.h"
 #include "btBulletFile.h"
+#include "LinearMath/btSerializer.h"
 #include "bCommon.h"
 #include <map>
 #include <vector>
 #include <string.h>
 
-bool isBulletFile = false;
+bool isBulletFile = true;
 
 using namespace bParse;
 typedef std::string bString;
@@ -216,10 +217,12 @@ int main(int argc,char** argv)
 	using namespace bParse;
 	dump = fopen("dump.py", "w");
 
+
 	if (!dump) return 0;
 	fprintf(dump, "%s\n", data);
 
-
+
+#if 0
 	char* filename = "../../../../data/r2d2_multibody.bullet";
 
 	if (argc==2)
@@ -269,9 +272,12 @@ int main(int argc,char** argv)
 		bBlenderFile	f(memBuf,len);
 		swap = (f.getFlags() & FD_ENDIAN_SWAP)!=0;
 	}
-
-
-
+#else
+	isBulletFile = true;
+	bool swap = false;
+	char* memBuf = sBulletDNAstr;
+	int len = sBulletDNAlen;
+#endif
 
 
 	char *blenderData = memBuf;

Extras/Serialize/HeaderGenerator/bulletGenerate.py

@@ -1,6 +1,9 @@
+import sys
+sys.path.append(".")
 import dump
 
 
+
 header = """/* Copyright (C) 2011 Erwin Coumans & Charlie C
 *
 * This software is provided 'as-is', without any express or implied

Extras/Serialize/makesdna/CMakeLists.txt

@@ -23,15 +23,29 @@ SET (INC_FILES
 SET(SRC makesdna.cpp)
 ADD_EXECUTABLE(makesdna ${SRC} ${INC_FILES})
 
-# Output BulletDNA.c
-ADD_CUSTOM_COMMAND(
-	OUTPUT ${BULLET_PHYSICS_SOURCE_DIR}/src/LinearMath/btSerializer.cpp
-	COMMAND ${CMAKE_CFG_INTDIR}/makesdna ${BULLET_PHYSICS_SOURCE_DIR}/src/LinearMath/btSerializer.cpp ${BULLET_PHYSICS_SOURCE_DIR}/Extras/Serialize/CommonSerialize/
-	DEPENDS makesdna
-)
+IF (CMAKE_CL_64)
+	# Output BulletDNA.c
+	ADD_CUSTOM_COMMAND(
+		OUTPUT ${BULLET_PHYSICS_SOURCE_DIR}/src/LinearMath/btSerializer64.cpp
+		COMMAND ${CMAKE_CFG_INTDIR}/makesdna ${BULLET_PHYSICS_SOURCE_DIR}/src/LinearMath/btSerializer64.cpp ${BULLET_PHYSICS_SOURCE_DIR}/Extras/Serialize/CommonSerialize/
+		DEPENDS makesdna
+	)
+SET(SRC  ${BULLET_PHYSICS_SOURCE_DIR}/src/LinearMath/btSerializer64.cpp)
+
+ELSE()
+	# Output BulletDNA.c
+	ADD_CUSTOM_COMMAND(
+		OUTPUT ${BULLET_PHYSICS_SOURCE_DIR}/src/LinearMath/btSerializer.cpp
+		COMMAND ${CMAKE_CFG_INTDIR}/makesdna ${BULLET_PHYSICS_SOURCE_DIR}/src/LinearMath/btSerializer.cpp ${BULLET_PHYSICS_SOURCE_DIR}/Extras/Serialize/CommonSerialize/
+		DEPENDS makesdna
+	)
+SET(SRC  ${BULLET_PHYSICS_SOURCE_DIR}/src/LinearMath/btSerializer.cpp)
+
+ENDIF()
 
 # Build bf_dna library
-SET(SRC  ${BULLET_PHYSICS_SOURCE_DIR}/src/LinearMath/btSerializer.cpp)
+
+
 ADD_LIBRARY(BulletDNA ${SRC} ${INC_FILES})
 
 MESSAGE(STATUS "Configuring makesdna")

VERSION

@@ -1 +1 @@
-2.86
+2.87

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp

@@ -1398,12 +1398,14 @@ void BulletMJCFImporter::getLinkChildIndices(int urdfLinkIndex, btAlignedObjectA
 	}
 }
 
-bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
+bool BulletMJCFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const
 {
-    jointLowerLimit = 0.f;
+	jointLowerLimit = 0.f;
     jointUpperLimit = 0.f;
 	jointDamping = 0.f;
 	jointFriction = 0.f;
+	jointMaxForce = 0;
+	jointMaxVelocity = 0;
 
 	const UrdfLink* link = m_data->getLink(m_data->m_activeModel,urdfLinkIndex);
 	if (link)
@@ -1421,7 +1423,9 @@ bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
 			jointUpperLimit = pj->m_upperLimit;
 			jointDamping = pj->m_jointDamping;
 			jointFriction = pj->m_jointFriction;
-
+			jointMaxForce = pj->m_effortLimit;
+			jointMaxVelocity = pj->m_velocityLimit;
+
 			return true;
 		} else
 		{
@@ -1432,6 +1436,14 @@ bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
 
 	return false;
 }
+
+bool BulletMJCFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
+{
+	//backwards compatibility for custom file importers
+	btScalar jointMaxForce = 0;
+	btScalar jointMaxVelocity = 0;
+	return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction,jointMaxForce, jointMaxVelocity);
+}
 
 bool BulletMJCFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const
 {

examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h

@@ -60,7 +60,8 @@ class BulletMJCFImporter : public URDFImporterInterface
     virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
 
     virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
-
+	virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
+
     virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
 
 	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -360,12 +360,14 @@ void  BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
 	}
 }
 
-bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
+bool BulletURDFImporter::getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const 
 {
     jointLowerLimit = 0.f;
     jointUpperLimit = 0.f;
 	jointDamping = 0.f;
 	jointFriction = 0.f;
+	jointMaxForce = 0.f;
+	jointMaxVelocity = 0.f;
 
 	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
 	btAssert(linkPtr);
@@ -384,7 +386,8 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
 			jointUpperLimit = pj->m_upperLimit;
 			jointDamping = pj->m_jointDamping;
 			jointFriction = pj->m_jointFriction;
-
+			jointMaxForce = pj->m_effortLimit;
+			jointMaxVelocity = pj->m_velocityLimit;
 			return true;
 		} else
 		{
@@ -394,6 +397,14 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
 	}
 
 	return false;
+
+};
+
+bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
+{
+	btScalar jointMaxForce;
+	btScalar jointMaxVelocity;
+	return getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction,jointMaxForce,jointMaxVelocity); 
 
 }
 

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -46,7 +46,8 @@ class BulletURDFImporter : public URDFImporterInterface
     virtual void  getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
 
     virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
-
+    virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
+
     virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
 
     virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -232,9 +232,11 @@ void ConvertURDF2BulletInternal(
     btScalar jointUpperLimit;
     btScalar jointDamping;
     btScalar jointFriction;
+	btScalar jointMaxForce;
+	btScalar jointMaxVelocity;
 
 
-    bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
+    bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction,jointMaxForce,jointMaxVelocity);
 	std::string linkName = u2b.getLinkName(urdfLinkIndex);
 
     if (flags & CUF_USE_SDF)
@@ -375,7 +377,10 @@ void ConvertURDF2BulletInternal(
                                                                   -offsetInB.getOrigin(),
                                                                   disableParentCollision);
                         cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
-                        cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
+                        cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction;
+						cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit;
+						cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit;
+
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
                         if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
                           //std::string name = u2b.getLinkName(urdfLinkIndex);

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -43,6 +43,14 @@ class URDFImporterInterface
     virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
 
     virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
+
+	virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const 
+	{
+		//backwards compatibility for custom file importers
+		jointMaxForce = 0;
+		jointMaxVelocity = 0;
+		return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction);
+	};
 
     virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0;
 

examples/RobotSimulator/CMakeLists.txt

@@ -14,6 +14,13 @@ SET(RobotSimulator_SRCS
 		b3RobotSimulatorClientAPI.h	
 		MinitaurSetup.cpp
 		MinitaurSetup.h
+		Bound.cpp
+		GRAPI/BulletMinitaur.cpp
+		GRAPI/BulletMotor.cpp
+		GRAPI/BulletRemoteKeyboard
+		GRAPI/BulletRobot
+		GRAPI/Impl.cpp
+		GRAPI/MinitaurLeg.cpp
 		../../examples/SharedMemory/IKTrajectoryHelper.cpp
 		../../examples/SharedMemory/IKTrajectoryHelper.h
 		../../examples/ExampleBrowser/InProcessExampleBrowser.cpp

examples/SharedMemory/BodyJointInfoUtility.h

@@ -60,7 +60,11 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
 
 			info.m_jointType = mb->m_links[link].m_jointType;
 			info.m_jointDamping = mb->m_links[link].m_jointDamping;
-			info.m_jointFriction = mb->m_links[link].m_jointFriction;
+			info.m_jointFriction1 = mb->m_links[link].m_jointFriction;
+			info.m_jointLowerLimit = mb->m_links[link].m_jointLowerLimit;
+			info.m_jointUpperLimit = mb->m_links[link].m_jointUpperLimit;
+			info.m_jointMaxForce = mb->m_links[link].m_jointMaxForce;
+			info.m_jointMaxVelocity = mb->m_links[link].m_jointMaxVelocity;
 
 			if ((mb->m_links[link].m_jointType == eRevoluteType) ||
 				(mb->m_links[link].m_jointType == ePrismaticType)) {

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -548,10 +548,24 @@ b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandl
     struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
     b3Assert(command);
     command->m_type =CMD_REQUEST_ACTUAL_STATE;
+	command->m_updateFlags = 0;
 	command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = bodyUniqueId;
     return (b3SharedMemoryCommandHandle) command;
 }
 
+int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+	b3Assert(command);
+	btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE);
+	if (computeLinkVelocity && command->m_type == CMD_REQUEST_ACTUAL_STATE)
+	{
+		command->m_updateFlags |= ACTUAL_STATE_COMPUTE_LINKVELOCITY;
+	}
+	return 0;
+}
+
+
 int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
 {
   const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -605,6 +619,8 @@ int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
     {
       state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
       state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + i];
+	  state->m_worldLinearVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6*linkIndex+i];
+	  state->m_worldAngularVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6*linkIndex+i+3];
     }
     for (int i = 0; i < 4; ++i) 
     {
@@ -1086,6 +1102,7 @@ int	b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemory
 
 b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
 {
+	B3_PROFILE("b3SubmitClientCommandAndWaitStatus");
 	b3Clock clock;
 	double startTime = clock.getTimeInSeconds();
 
@@ -1099,11 +1116,17 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
 
 		double timeOutInSeconds = cl->getTimeOut();
 
-		b3SubmitClientCommand(physClient, commandHandle);
-
-		while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
 		{
-			statusHandle = b3ProcessServerStatus(physClient);
+			B3_PROFILE("b3SubmitClientCommand");
+			b3SubmitClientCommand(physClient, commandHandle);
+		}
+		{
+			B3_PROFILE("b3ProcessServerStatus");
+			while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
+			{
+					clock.usleep(0);
+					statusHandle = b3ProcessServerStatus(physClient);
+			}
 		}
 		return (b3SharedMemoryStatusHandle)statusHandle;
 	}

examples/SharedMemory/PhysicsClientC_API.h

@@ -313,6 +313,8 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle c
 int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
 
 b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
+int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity);
+
 int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
 int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);