diff --git a/data/two_cubes.sdf b/data/two_cubes.sdf
index 24c0854bd..10dce545e 100644
--- a/data/two_cubes.sdf
+++ b/data/two_cubes.sdf
@@ -86,6 +86,7 @@
0
+ 1
0.512455 -1.58317 0.5 0 -0 0
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index 75e16b8f7..b6004aeef 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/examples/ExampleBrowser/ExampleEntries.cpp
@@ -242,7 +242,10 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
- ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc),
+ ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
+ ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
+
+
#ifdef ENABLE_LUA
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 31b8df6b9..914c9d091 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -5,6 +5,29 @@
#include "SharedMemoryCommands.h"
+b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
+{
+ PhysicsClient* cl = (PhysicsClient* ) physClient;
+ b3Assert(cl);
+ b3Assert(cl->canSubmitCommand());
+
+ struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+ b3Assert(command);
+ command->m_type = CMD_LOAD_SDF;
+ int len = strlen(sdfFileName);
+ if (lenm_sdfArguments.m_sdfFileName,sdfFileName);
+ } else
+ {
+ command->m_sdfArguments.m_sdfFileName[0] = 0;
+ }
+ command->m_updateFlags = SDF_ARGS_FILE_NAME;
+
+ return (b3SharedMemoryCommandHandle) command;
+}
+
+
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
@@ -146,6 +169,8 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
+
return 0;
}
@@ -154,6 +179,7 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
return 0;
}
@@ -162,6 +188,8 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
+
return 0;
}
@@ -170,6 +198,7 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
return 0;
}
@@ -179,6 +208,7 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
+ command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0;
}
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index b202daa35..37008aceb 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -88,6 +88,8 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
+b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
+
///Set joint control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp
index a57fde289..15c90748a 100644
--- a/examples/SharedMemory/PhysicsClientExample.cpp
+++ b/examples/SharedMemory/PhysicsClientExample.cpp
@@ -156,16 +156,12 @@ protected:
{
for (int i=0;i=0))
+ {
+ m_canvas->destroyCanvas(m_canvasIndex);
+ }
b3Printf("~PhysicsClientExample\n");
}
@@ -563,10 +564,15 @@ void PhysicsClientExample::initPhysics()
m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
- m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
- //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
- //m_physicsClientHandle = b3ConnectPhysicsDirect();
-
+ if (m_options == eCLIENTEXAMPLE_DIRECT)
+ {
+ m_physicsClientHandle = b3ConnectPhysicsDirect();
+ } else
+ {
+ m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
+ //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
+ }
+
if (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3Warning("Cannot connect to physics client");
diff --git a/examples/SharedMemory/PhysicsClientExample.h b/examples/SharedMemory/PhysicsClientExample.h
index b564d01be..a36a37e75 100644
--- a/examples/SharedMemory/PhysicsClientExample.h
+++ b/examples/SharedMemory/PhysicsClientExample.h
@@ -4,7 +4,7 @@
enum ClientExampleOptions
{
eCLIENTEXAMPLE_LOOPBACK=1,
- eCLIENTEAXMPLE_DIRECT=2,
+ eCLIENTEXAMPLE_DIRECT=2,
eCLIENTEXAMPLE_SERVER=3,
};
diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp
index 72b5eb65e..37baeaa83 100644
--- a/examples/SharedMemory/PhysicsDirect.cpp
+++ b/examples/SharedMemory/PhysicsDirect.cpp
@@ -33,6 +33,12 @@ struct PhysicsDirectInternalData
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
+ int m_cachedCameraPixelsWidth;
+ int m_cachedCameraPixelsHeight;
+ btAlignedObjectArray m_cachedCameraPixelsRGBA;
+ btAlignedObjectArray m_cachedCameraDepthBuffer;
+
+
PhysicsServerCommandProcessor* m_commandProcessor;
PhysicsDirectInternalData()
@@ -167,6 +173,73 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
return m_data->m_hasStatus;
}
+bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
+{
+ SharedMemoryCommand command = orgCommand;
+
+ const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+ do
+ {
+
+ bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+ m_data->m_hasStatus = hasStatus;
+ if (hasStatus)
+ {
+ btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED);
+
+ if (m_data->m_verboseOutput)
+ {
+ b3Printf("Camera image OK\n");
+ }
+
+ int numBytesPerPixel = 4;//RGBA
+ int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
+ serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
+ serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
+
+ m_data->m_cachedCameraPixelsWidth = 0;
+ m_data->m_cachedCameraPixelsHeight = 0;
+
+ int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight;
+
+ m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
+ m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
+ m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
+
+
+ unsigned char* rgbaPixelsReceived =
+ (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
+ printf("pixel = %d\n", rgbaPixelsReceived[0]);
+
+ for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
+ = rgbaPixelsReceived[i];
+ }
+
+ if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
+ {
+
+
+ // continue requesting remaining pixels
+ command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
+ command.m_requestPixelDataArguments.m_startPixelIndex =
+ serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
+ serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
+ } else
+ {
+ m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
+ m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
+ }
+ }
+ } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0);
+
+ return m_data->m_hasStatus;
+
+
+}
+
bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
{
if (command.m_type==CMD_REQUEST_DEBUG_LINES)
@@ -174,6 +247,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
return processDebugLines(command);
}
+ if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA)
+ {
+ return processCamera(command);
+ }
+
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
@@ -333,9 +411,9 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
{
if (cameraData)
{
- cameraData->m_pixelHeight = 0;
- cameraData->m_pixelWidth = 0;
- cameraData->m_depthValues = 0;
- cameraData->m_rgbColorData = 0;
+ cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
+ cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
+ cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
+ cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
}
}
diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h
index fb1aeefa6..9dea925d3 100644
--- a/examples/SharedMemory/PhysicsDirect.h
+++ b/examples/SharedMemory/PhysicsDirect.h
@@ -19,6 +19,8 @@ protected:
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
+ bool processCamera(const struct SharedMemoryCommand& orgCommand);
+
public:
PhysicsDirect();
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 1fc4efdcc..b4317bcc6 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -1192,12 +1192,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
mb->clearForcesAndTorques();
int torqueIndex = 0;
- btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
- clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
- clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
- btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
- clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
- clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
+ btVector3 f(0,0,0);
+ btVector3 t(0,0,0);
+
+ if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+ {
+ f = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
+ t = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
+ clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
+ }
torqueIndex+=6;
mb->addBaseForce(f);
mb->addBaseTorque(t);
@@ -1206,7 +1212,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
for (int dof=0;dofgetLink(link).m_dofCount;dof++)
{
- double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
+ double torque = 0.f;
+ if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+ torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
mb->addJointTorqueMultiDof(link,dof,torque);
torqueIndex++;
}
@@ -1233,10 +1241,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (motorPtr)
{
btMultiBodyJointMotor* motor = *motorPtr;
- btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
+ btScalar desiredVelocity = 0.f;
+ if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0)
+ desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity);
- btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
+ btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
+ if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+ maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
+
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
@@ -1247,6 +1260,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
break;
}
+
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
if (m_data->m_verboseOutput)
@@ -1271,11 +1285,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
btMultiBodyJointMotor* motor = *motorPtr;
- btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
- btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
-
- btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
- btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
+ btScalar desiredVelocity = 0.f;
+ if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0)
+ desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
+ btScalar desiredPosition = 0.f;
+ if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_Q)!=0)
+ desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
+
+ btScalar kp = 0.f;
+ if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KP)!=0)
+ kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
+ btScalar kd = 0.f;
+ if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KD)!=0)
+ kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
int dof1 = 0;
btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1];
@@ -1288,9 +1310,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
motor->setVelocityTarget(desiredVelocity);
- btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
+ btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
+
+ if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+ maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
- motor->setMaxAppliedImpulse(1000);//maxImp);
+ motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 42436d2b0..8328b485a 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -30,6 +30,7 @@
#define MAX_DEGREE_OF_FREEDOM 64
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
+#define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
@@ -54,6 +55,16 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z)
return tmp;
}
+enum EnumSdfArgsUpdateFlags
+{
+ SDF_ARGS_FILE_NAME=1,
+};
+
+struct SdfArgs
+{
+ char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
+};
+
enum EnumUrdfArgsUpdateFlags
{
URDF_ARGS_FILE_NAME=1,
@@ -179,6 +190,15 @@ struct SendDesiredStateArgs
};
+enum EnumSimDesiredStateUpdateFlags
+{
+ SIM_DESIRED_STATE_HAS_Q=1,
+ SIM_DESIRED_STATE_HAS_QDOT=2,
+ SIM_DESIRED_STATE_HAS_KD=4,
+ SIM_DESIRED_STATE_HAS_KP=8,
+ SIM_DESIRED_STATE_HAS_MAX_FORCE=16,
+};
+
enum EnumSimParamUpdateFlags
{
@@ -287,6 +307,7 @@ struct SharedMemoryCommand
union
{
struct UrdfArgs m_urdfArguments;
+ struct SdfArgs m_sdfArguments;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments;
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index f7d04f724..2f676e057 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -5,7 +5,8 @@
enum EnumSharedMemoryClientCommand
{
- CMD_LOAD_URDF,
+ CMD_LOAD_SDF,
+ CMD_LOAD_URDF,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
// CMD_DELETE_BOX_COLLISION_SHAPE,
@@ -35,7 +36,8 @@ enum EnumSharedMemoryServerStatus
CMD_CLIENT_COMMAND_COMPLETED,
//the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
CMD_UNKNOWN_COMMAND_FLUSHED,
-
+ CMD_SDF_LOADING_COMPLETED,
+ CMD_SDF_LOADING_FAILED,
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,