make one of the cubes in two_cubes.sdf static (immovable) using the <static> tag in SDF

add an example using 'direct'
fix the send-desired-state commands, to add flags for arguments set, using default values.
Start exposing SDF loading in shared memory api, not fully implemented yet.

data/two_cubes.sdf

@@ -86,6 +86,7 @@
       <heading_deg>0</heading_deg>
     </spherical_coordinates>
     <model name='unit_box_0'>
+ <static>1</static>
       <pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
       <link name='unit_box_0::link'>
         <inertial>

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

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 (len<MAX_SDF_FILENAME_LENGTH)
+	{
+		strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
+	} else
+	{
+		command->m_sdfArguments.m_sdfFileName[0] = 0;
+	}
+	command->m_updateFlags = SDF_ARGS_FILE_NAME;
+	
+	return (b3SharedMemoryCommandHandle) command;
+}
+
+
 
 b3SharedMemoryCommandHandle	b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
 {
@@ -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;
 }
 

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);

examples/SharedMemory/PhysicsClientExample.cpp

@@ -156,16 +156,12 @@ protected:
 	{
         for (int i=0;i<m_numMotors;i++)
         {
-            // btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
-            // int uIndex = m_motorTargetVelocities[i].m_uIndex;
-            // b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
             
             btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
             int qIndex = m_motorTargetPositions[i].m_qIndex;
             int uIndex = m_motorTargetPositions[i].m_uIndex;
             b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
             b3JointControlSetKp(commandHandle, uIndex, 0.1);
-            b3JointControlSetKd(commandHandle, uIndex, 0.0);
             
             b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
         }
@@ -430,6 +426,11 @@ PhysicsClientExample::~PhysicsClientExample()
 		bool deInitializeSharedMemory = true;
 		m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
 	}
+	
+	if (m_canvas && (m_canvasIndex>=0))
+	{
+		m_canvas->destroyCanvas(m_canvasIndex);
+	}
     b3Printf("~PhysicsClientExample\n");
 }
 
@@ -563,9 +564,14 @@ void	PhysicsClientExample::initPhysics()
         m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
     }
     
+	if (m_options == eCLIENTEXAMPLE_DIRECT)
+	{
+		m_physicsClientHandle = b3ConnectPhysicsDirect();
+	} else
+	{
 	    m_physicsClientHandle  = b3ConnectSharedMemory(m_sharedMemoryKey);
 		//m_physicsClientHandle  = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
-	//m_physicsClientHandle = b3ConnectPhysicsDirect();
+	}
 	
     if (!b3CanSubmitCommand(m_physicsClientHandle))
     {

examples/SharedMemory/PhysicsClientExample.h

@@ -4,7 +4,7 @@
 enum ClientExampleOptions
 {
 	eCLIENTEXAMPLE_LOOPBACK=1,
-	eCLIENTEAXMPLE_DIRECT=2,
+	eCLIENTEXAMPLE_DIRECT=2,
 	eCLIENTEXAMPLE_SERVER=3,
 };
 

examples/SharedMemory/PhysicsDirect.cpp

@@ -33,6 +33,12 @@ struct PhysicsDirectInternalData
 	
 	char    m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
 
+	int m_cachedCameraPixelsWidth;
+	int m_cachedCameraPixelsHeight;
+	btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
+	btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
+
+
 	PhysicsServerCommandProcessor* m_commandProcessor;
 
 	PhysicsDirectInternalData()
@@ -167,6 +173,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;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
+			{
+				m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] 
+					= rgbaPixelsReceived[i];
+			}
+
+			if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
+			{
+				
+
+				// continue requesting remaining pixels
+				command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
+				command.m_requestPixelDataArguments.m_startPixelIndex = 
+					serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + 
+					serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
+			} else
+			{
+				m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
+				m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
+			}	
+		}
+	}  while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0);
+	
+	return m_data->m_hasStatus;
+
+
+}
+
 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;
 	}
 }

examples/SharedMemory/PhysicsDirect.h

@@ -19,6 +19,8 @@ protected:
 
 	bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
 
+	bool processCamera(const struct SharedMemoryCommand& orgCommand);
+
 public:
 
     PhysicsDirect();

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],
+										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]);
-                                        btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
+											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;dof<mb->getLink(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 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 = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
-                                                        btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
+                                                        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;
 
-                                                        motor->setMaxAppliedImpulse(1000);//maxImp);
+														if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
+															maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
+                                                        
+                                                        motor->setMaxAppliedImpulse(maxImp);
                                                         numMotors++;
                                                     }
 

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;

examples/SharedMemory/SharedMemoryPublic.h

@@ -5,6 +5,7 @@
 
 enum EnumSharedMemoryClientCommand
 {
+    CMD_LOAD_SDF,
 	CMD_LOAD_URDF,
         CMD_SEND_BULLET_DATA_STREAM,
         CMD_CREATE_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,