void	RobotControlExample::stepSimulation(float deltaTime)
{
    m_physicsServer.processClientCommands();

	if (m_physicsClient.isConnected())
    {
		
		SharedMemoryStatus status;
		bool hasStatus = m_physicsClient.processServerStatus(status);
		if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
		{
			for (int i=0;i<m_physicsClient.getNumJoints();i++)
			{
				b3JointInfo info;
				m_physicsClient.getJointInfo(i,info);
				b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
				
                if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
                {
                    if (m_numMotors<MAX_NUM_MOTORS)
                    {
                        char motorName[1024];
                        sprintf(motorName,"%s q'", info.m_jointName);
                        MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors];
                        motorInfo->m_velTarget = 0.f;
                        motorInfo->m_uIndex = info.m_uIndex;
                    
                        SliderParams slider(motorName,&motorInfo->m_velTarget);
                        slider.m_minVal=-4;
                        slider.m_maxVal=4;
                        m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                        m_numMotors++;
                    }
                }
				
			}
		}

		
		if (m_physicsClient.canSubmitCommand())
		{
			if (m_userCommandRequests.size())
			{
				b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
				SharedMemoryCommand& cmd = m_userCommandRequests[0];

				//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
				for (int i=1;i<m_userCommandRequests.size();i++)
				{
					m_userCommandRequests[i-1] = m_userCommandRequests[i];
				}

				m_userCommandRequests.pop_back();
				m_physicsClient.submitClientCommand(cmd);
			}
		}
	}
}
	SampleJobInterface* consumeJob()
	{
	    SampleJobInterface* job = 0;
	     m_cs->lock();
	     int sz = m_jobQueue.size();
	     if (sz)
         {
            job = m_jobQueue[sz-1];
            m_jobQueue.pop_back();
         }
         m_cs->unlock();
	    return job;
	}
void	PhysicsClientExample::stepSimulation(float deltaTime)
{
    
	if (m_physicsClient.isConnected())
    {
		SharedMemoryStatus status;
		bool hasStatus = m_physicsClient.processServerStatus(status);
		if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
		{
			for (int i=0;i<m_physicsClient.getNumJoints();i++)
			{
				b3JointInfo info;
				m_physicsClient.getJointInfo(i,info);
				b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
				
			}
		
            if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
            {
                for (int i=0;i<m_physicsClient.getNumJoints();i++)
                {
                    b3JointInfo info;
                    m_physicsClient.getJointInfo(i,info);
                    b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
                    
                    if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
                    {
                        if (m_numMotors<MAX_NUM_MOTORS)
                        {
                            char motorName[1024];
                            sprintf(motorName,"%s q'", info.m_jointName);
                            MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
                            motorInfo->m_velTarget = 0.f;
                            motorInfo->m_uIndex = info.m_uIndex;
                            
                            SliderParams slider(motorName,&motorInfo->m_velTarget);
                            slider.m_minVal=-4;
                            slider.m_maxVal=4;
                            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                            m_numMotors++;
                        }
                    }
                    
                }

            }
		}
    
		if (hasStatus && status.m_type ==CMD_DEBUG_LINES_COMPLETED)
		{
            SharedMemoryCommand command;
			if (status.m_sendDebugLinesArgs.m_numRemainingDebugLines>0)
			{
				//continue requesting debug lines for drawing
				command.m_type =CMD_REQUEST_DEBUG_LINES;
				command.m_requestDebugLinesArguments.m_debugMode = btIDebugDraw::DBG_DrawWireframe;//DBG_DrawConstraints;
				command.m_requestDebugLinesArguments.m_startingLineIndex = status.m_sendDebugLinesArgs.m_numDebugLines+status.m_sendDebugLinesArgs.m_startingLineIndex;
				enqueueCommand(command);
			}
		}
		if (m_physicsClient.canSubmitCommand())
		{
			if (m_userCommandRequests.size())
			{
				//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
				SharedMemoryCommand command = m_userCommandRequests[0];

				//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
				for (int i=1;i<m_userCommandRequests.size();i++)
				{
					m_userCommandRequests[i-1] = m_userCommandRequests[i];
				}

				m_userCommandRequests.pop_back();
				
				//for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
				if (command.m_type==CMD_RESET_SIMULATION)
				{
					m_guiHelper->getParameterInterface()->removeAllParameters();
					m_numMotors=0;
					createButtons();
				}
				
				m_physicsClient.submitClientCommand(command);
			}  else
			{
				if (m_numMotors)
				{
					SharedMemoryCommand command;
					command.m_type =CMD_SEND_DESIRED_STATE;
					prepareControlCommand(command);
					enqueueCommand(command);		

					command.m_type =CMD_STEP_FORWARD_SIMULATION;
					enqueueCommand(command);

					command.m_type = CMD_REQUEST_ACTUAL_STATE;
					enqueueCommand(command);
				}
			}
		}
	}
	

}
void	RobotControlExample::stepSimulation(float deltaTime)
{
    
	m_physicsServer.processClientCommands();

	if (m_physicsClient.isConnected())
    {
		
		SharedMemoryStatus status;
		bool hasStatus = m_physicsClient.processServerStatus(status);
		if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
		{
			for (int i=0;i<m_physicsClient.getNumJoints();i++)
			{
				b3JointInfo info;
				m_physicsClient.getJointInfo(i,info);
				if (m_verboseOutput)
				{
					b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
				}
				
                if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
                {
                    if (m_numMotors<MAX_NUM_MOTORS)
                    {

						switch (m_option)
						{
						case ROBOT_VELOCITY_CONTROL:
							{
		                        char motorName[1024];
								sprintf(motorName,"%s q'", info.m_jointName);
								MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
								motorInfo->m_velTarget = 0.f;
								motorInfo->m_posTarget = 0.f;

								motorInfo->m_uIndex = info.m_uIndex;
                    
								SliderParams slider(motorName,&motorInfo->m_velTarget);
								slider.m_minVal=-4;
								slider.m_maxVal=4;
								m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
								m_numMotors++;
								break;
							}
						case ROBOT_PD_CONTROL:
						{
							char motorName[1024];
							
							MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
							motorInfo->m_velTarget = 0.f;
							motorInfo->m_posTarget = 0.f;
							motorInfo->m_uIndex = info.m_uIndex;
							motorInfo->m_posIndex  = info.m_qIndex;
                            motorInfo->m_kp = 1;
                            motorInfo->m_kd = 0;
                            
                            {
                                sprintf(motorName,"%s kp", info.m_jointName);
                                SliderParams slider(motorName,&motorInfo->m_kp);
                                slider.m_minVal=0;
                                slider.m_maxVal=1;
                                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                            }
                            
                            {
                                sprintf(motorName,"%s q", info.m_jointName);
							SliderParams slider(motorName,&motorInfo->m_posTarget);
							slider.m_minVal=-SIMD_PI;
							slider.m_maxVal=SIMD_PI;
							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                            }
                            {
                                sprintf(motorName,"%s kd", info.m_jointName);
                                SliderParams slider(motorName,&motorInfo->m_kd);
                                slider.m_minVal=0;
                                slider.m_maxVal=1;
                                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                            }
                           
                            {
                                 sprintf(motorName,"%s q'", info.m_jointName);
                            SliderParams slider(motorName,&motorInfo->m_velTarget);
                            slider.m_minVal=-10;
                            slider.m_maxVal=10;
                            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                            }
							m_numMotors++;
							break;
						}
						default:
							{
								b3Warning("Unknown control mode in RobotControlExample::stepSimulation");
							}
						};
                    }
                }
				
			}
		}

		
		if (m_physicsClient.canSubmitCommand())
		{
			if (m_userCommandRequests.size())
			{
				if (m_verboseOutput)
				{
					b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
				}
				SharedMemoryCommand cmd = m_userCommandRequests[0];

				//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
				for (int i=1;i<m_userCommandRequests.size();i++)
				{
					m_userCommandRequests[i-1] = m_userCommandRequests[i];
				}

				m_userCommandRequests.pop_back();
				if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
				{
					char relativeFileName[1024];
					
					bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
					if (fileFound)
					{
						FILE *fp = fopen(relativeFileName, "rb");
						if (fp)
						{
							fseek(fp, 0L, SEEK_END);
							int mFileLen = ftell(fp);
							fseek(fp, 0L, SEEK_SET);
							if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
							{
								char* data = (char*)malloc(mFileLen);

								fread(data, mFileLen, 1, fp);
								fclose(fp);
								cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
								m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
								if (m_verboseOutput)
								{
									b3Printf("Loaded bullet data chunks into shared memory\n");
								}
								free(data);
							} else
							{
								b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
							}
						} else
						{
							b3Warning("Cannot open file %s\n", relativeFileName);
						}
					} else
					{
						b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
					}

				}
				
				m_physicsClient.submitClientCommand(cmd);
			} else
			{

				if (m_numMotors)
				{
					SharedMemoryCommand command;
					command.m_type =CMD_SEND_DESIRED_STATE;
					prepareControlCommand(command);
					enqueueCommand(command);		

					command.m_type =CMD_STEP_FORWARD_SIMULATION;
					enqueueCommand(command);
				}

			}
		}
	}
}
void	RobotControlExample::stepSimulation(float deltaTime)
{
    
	m_physicsServer.processClientCommands();

	if (m_physicsClient.isConnected())
    {
		
		SharedMemoryStatus status;
		bool hasStatus = m_physicsClient.processServerStatus(status);
		if (hasStatus && status.m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
		{
			//update sensor feedback: joint force/torque data and measured joint positions
			
			for (int i=0;i<m_numMotors;i++)
			{
				int jointIndex = m_motorTargetState[i].m_jointIndex;
				int positionIndex = m_motorTargetState[i].m_posIndex;
				int velocityIndex = m_motorTargetState[i].m_uIndex;

				m_motorTargetState[i].m_measuredJointPosition = status.m_sendActualStateArgs.m_actualStateQ[positionIndex];
				m_motorTargetState[i].m_measuredJointVelocity = status.m_sendActualStateArgs.m_actualStateQdot[velocityIndex];
				m_motorTargetState[i].m_measuredJointForce.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex],
																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+1],
																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+2]);
				m_motorTargetState[i].m_measuredJointTorque.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+3],
																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+4],
																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+5]);
				
				if (m_motorTargetState[i].m_measuredJointPosition>0.1)
				{
					m_motorTargetState[i].m_velTarget = -1.5;
				} else
				{
					m_motorTargetState[i].m_velTarget = 1.5;
				}
				
				b3Printf("Joint Force (Linear) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointForce.x(),m_motorTargetState[i].m_measuredJointForce.y(),m_motorTargetState[i].m_measuredJointForce.z());
				b3Printf("Joint Torque (Angular) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointTorque.x(),m_motorTargetState[i].m_measuredJointTorque.y(),m_motorTargetState[i].m_measuredJointTorque.z());

			}

			
		}

		if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
		{
			SharedMemoryCommand sensorCommand;
			sensorCommand.m_type = CMD_CREATE_SENSOR;
			sensorCommand.m_createSensorArguments.m_numJointSensorChanges = 0;

			for (int jointIndex=0;jointIndex<m_physicsClient.getNumJoints();jointIndex++)
			{
				b3JointInfo info;
				m_physicsClient.getJointInfo(jointIndex,info);
				if (m_verboseOutput)
				{
					b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
				}
				
                if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
                {
                    if (m_numMotors<MAX_NUM_MOTORS)
                    {

						switch (m_option)
						{
						case ROBOT_VELOCITY_CONTROL:
							{
		                        char motorName[1024];
								sprintf(motorName,"%s q'", info.m_jointName);
								MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
								motorInfo->m_jointName = info.m_jointName;

								motorInfo->m_velTarget = 0.f;
								motorInfo->m_posTarget = 0.f;

								motorInfo->m_uIndex = info.m_uIndex;
                    
								SliderParams slider(motorName,&motorInfo->m_velTarget);
								slider.m_minVal=-4;
								slider.m_maxVal=4;
								m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
								m_numMotors++;
								break;
							}
						case ROBOT_PD_CONTROL:
						{
							char motorName[1024];
							
							MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
							motorInfo->m_jointName = info.m_jointName;
							motorInfo->m_velTarget = 0.f;
							motorInfo->m_posTarget = 0.f;
							motorInfo->m_uIndex = info.m_uIndex;
							motorInfo->m_posIndex  = info.m_qIndex;
                            motorInfo->m_kp = 1;
                            motorInfo->m_kd = 0;
                            
                            {
                                sprintf(motorName,"%s kp", info.m_jointName);
                                SliderParams slider(motorName,&motorInfo->m_kp);
                                slider.m_minVal=0;
                                slider.m_maxVal=1;
                                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                            }
                            
                            {
                                sprintf(motorName,"%s q", info.m_jointName);
							SliderParams slider(motorName,&motorInfo->m_posTarget);
							slider.m_minVal=-SIMD_PI;
							slider.m_maxVal=SIMD_PI;
							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                            }
                            {
                                sprintf(motorName,"%s kd", info.m_jointName);
                                SliderParams slider(motorName,&motorInfo->m_kd);
                                slider.m_minVal=0;
                                slider.m_maxVal=1;
                                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                            }
                           
                            {
                                 sprintf(motorName,"%s q'", info.m_jointName);
                            SliderParams slider(motorName,&motorInfo->m_velTarget);
                            slider.m_minVal=-10;
                            slider.m_maxVal=10;
                            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
                            }
							m_numMotors++;
							break;
						}
						case ROBOT_PING_PONG_JOINT_FEEDBACK:
						{
								
							if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
							{
								if (m_numMotors<MAX_NUM_MOTORS)
								{
									MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
									motorInfo->m_jointName = info.m_jointName;
									motorInfo->m_velTarget = 0.f;
									motorInfo->m_posTarget = 0.f;
									motorInfo->m_uIndex = info.m_uIndex;
									motorInfo->m_posIndex  = info.m_qIndex;
									motorInfo->m_jointIndex = jointIndex;
									sensorCommand.m_createSensorArguments.m_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex;
									sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true;
									sensorCommand.m_createSensorArguments.m_numJointSensorChanges++;
									m_numMotors++;
								}
							}
						
							

							 break;
						}
						default:
							{
								b3Warning("Unknown control mode in RobotControlExample::stepSimulation");
							}
						};
                    }
                }
				
			}
			

			if (sensorCommand.m_createSensorArguments.m_numJointSensorChanges)
			{
				enqueueCommand(sensorCommand);
			}

		}

		
		
		if (m_physicsClient.canSubmitCommand())
		{
			if (m_userCommandRequests.size())
			{
				if (m_verboseOutput)
				{
					b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
				}
				SharedMemoryCommand cmd = m_userCommandRequests[0];

				//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
				for (int i=1;i<m_userCommandRequests.size();i++)
				{
					m_userCommandRequests[i-1] = m_userCommandRequests[i];
				}

				m_userCommandRequests.pop_back();
				if (cmd.m_type == CMD_CREATE_SENSOR)
				{
					b3Printf("CMD_CREATE_SENSOR!\n");
				}
				if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
				{
					char relativeFileName[1024];
					
					bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
					if (fileFound)
					{
						FILE *fp = fopen(relativeFileName, "rb");
						if (fp)
						{
							fseek(fp, 0L, SEEK_END);
							int mFileLen = ftell(fp);
							fseek(fp, 0L, SEEK_SET);
							if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
							{
								char* data = (char*)malloc(mFileLen);

								fread(data, mFileLen, 1, fp);
								fclose(fp);
								cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
								m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
								if (m_verboseOutput)
								{
									b3Printf("Loaded bullet data chunks into shared memory\n");
								}
								free(data);
							} else
							{
								b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
							}
						} else
						{
							b3Warning("Cannot open file %s\n", relativeFileName);
						}
					} else
					{
						b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
					}

				}
				
				m_physicsClient.submitClientCommand(cmd);
			} else
			{

				if (m_numMotors)
				{
					SharedMemoryCommand command;
					command.m_type =CMD_SEND_DESIRED_STATE;
					prepareControlCommand(command);
					enqueueCommand(command);		

					command.m_type =CMD_STEP_FORWARD_SIMULATION;
					enqueueCommand(command);

					command.m_type = CMD_REQUEST_ACTUAL_STATE;
					enqueueCommand(command);
				}

			}
		}
	}
}
void	PhysicsClientExample::stepSimulation(float deltaTime)
{

	if (m_options == eCLIENTEXAMPLE_SERVER)
	{
		for (int i=0;i<100;i++)
		{
			m_physicsServer.processClientCommands();
		}
	}

	if (m_prevSelectedBody != m_selectedBody)
	{
		createButtons();
		m_prevSelectedBody = m_selectedBody;
	}
    
	//while (!b3CanSubmitCommand(m_physicsClientHandle))
	{
		b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
		bool hasStatus = (status != 0);
		if (hasStatus)
		{

			int statusType = b3GetStatusType(status);
			if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
			{
				//b3Printf("bla\n");
			}
			if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
            {
				static int counter=0;
				char msg[1024];
				sprintf(msg,"Camera image %d OK\n",counter++);
				b3CameraImageData imageData;
				b3GetCameraImageData(m_physicsClientHandle,&imageData);
				if (m_canvas && m_canvasIndex >=0)
				{
					for (int i=0;i<imageData.m_pixelWidth;i++)
					{
						for (int j=0;j<imageData.m_pixelHeight;j++)
						{
                            int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
                            int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
							btClamp(yIndex,0,imageData.m_pixelHeight);
							btClamp(xIndex,0,imageData.m_pixelWidth);
							int bytesPerPixel = 4;
							
							int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
							m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
                                               
									imageData.m_rgbColorData[pixelIndex],
									imageData.m_rgbColorData[pixelIndex+1],
									imageData.m_rgbColorData[pixelIndex+2],
                                               255);
//									imageData.m_rgbColorData[pixelIndex+3]);
						}
					}
					m_canvas->refreshImageData(m_canvasIndex);
				}

                b3Printf(msg);
            } 
            if (statusType == CMD_CAMERA_IMAGE_FAILED)
            {
                b3Printf("Camera image FAILED\n");
            }
       
        
      		if (statusType == CMD_URDF_LOADING_COMPLETED)
			{
				int bodyIndex = b3GetStatusBodyIndex(status);
				if (bodyIndex>=0)
				{
					int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
            
					for (int i=0;i<numJoints;i++)
					{
						b3JointInfo info;
						b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
						b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
				
					}
					ComboBoxParams comboParams;
					comboParams.m_comboboxId = bodyIndex;
					comboParams.m_numItems = 1;
					comboParams.m_startItem = 0;
					comboParams.m_callback = MyComboBoxCallback;
					comboParams.m_userPointer = this;
					const char* bla = "bla";
					const char* blarray[1];
					blarray[0] = bla;
				
					comboParams.m_items=blarray;//{&bla};
					m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
		

				}

			}
    
		}
	}
    if (b3CanSubmitCommand(m_physicsClientHandle))
    {
        if (m_userCommandRequests.size())
        {
            //b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
            int commandId = m_userCommandRequests[0];

            //a manual 'pop_front', we don't use 'remove' because it will re-order the commands
            for (int i=1;i<m_userCommandRequests.size();i++)
            {
                m_userCommandRequests[i-1] = m_userCommandRequests[i];
            }

            m_userCommandRequests.pop_back();
            
            //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
            if (commandId ==CMD_RESET_SIMULATION)
            {
				m_selectedBody = -1;
                m_numMotors=0;
                createButtons();
				b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
				if (m_options == eCLIENTEXAMPLE_SERVER)
				{
					b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
					while (!b3CanSubmitCommand(m_physicsClientHandle))
					{
						m_physicsServer.processClientCommands();
						b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
						bool hasStatus = (status != 0);
						if (hasStatus)
						{
							int statusType = b3GetStatusType(status);
							b3Printf("Status after reset: %d",statusType);
						}
					}
				} else
				{
					prepareAndSubmitCommand(commandId);
				}
            } else
			{
	            prepareAndSubmitCommand(commandId);
			}
            
        }  else
        {
            if (m_numMotors)
            {
                enqueueCommand(CMD_SEND_DESIRED_STATE);
                enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
				if (m_options != eCLIENTEXAMPLE_SERVER)
				{
					enqueueCommand(CMD_REQUEST_DEBUG_LINES);
				}
                //enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
            }
        }
    }


}
void	PhysicsClientExample::stepSimulation(float deltaTime)
{
	if (m_prevSelectedBody != m_selectedBody)
	{
		createButtons();
		m_prevSelectedBody = m_selectedBody;
	}
    
	//while (!b3CanSubmitCommand(m_physicsClientHandle))
	{
		b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
		bool hasStatus = (status != 0);
		if (hasStatus)
		{

			int statusType = b3GetStatusType(status);
			if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
			{
				//b3Printf("bla\n");
			}
      		if (statusType == CMD_URDF_LOADING_COMPLETED)
			{
				int bodyIndex = b3GetStatusBodyIndex(status);
				if (bodyIndex>=0)
				{
					int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
            
					for (int i=0;i<numJoints;i++)
					{
						b3JointInfo info;
						b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
						b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
				
					}
					ComboBoxParams comboParams;
					comboParams.m_comboboxId = bodyIndex;
					comboParams.m_numItems = 1;
					comboParams.m_startItem = 0;
					comboParams.m_callback = MyComboBoxCallback;
					comboParams.m_userPointer = this;
					const char* bla = "bla";
					const char* blarray[1];
					blarray[0] = bla;
				
					comboParams.m_items=blarray;//{&bla};
					m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
		

				}

			}
    
		}
	}
    if (b3CanSubmitCommand(m_physicsClientHandle))
    {
        if (m_userCommandRequests.size())
        {
            //b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
            int commandId = m_userCommandRequests[0];

            //a manual 'pop_front', we don't use 'remove' because it will re-order the commands
            for (int i=1;i<m_userCommandRequests.size();i++)
            {
                m_userCommandRequests[i-1] = m_userCommandRequests[i];
            }

            m_userCommandRequests.pop_back();
            
            //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
            if (commandId ==CMD_RESET_SIMULATION)
            {
				m_selectedBody = -1;
                m_numMotors=0;
                createButtons();
            }
			
            
            prepareAndSubmitCommand(commandId);
            
        }  else
        {
            if (m_numMotors)
            {
                enqueueCommand(CMD_SEND_DESIRED_STATE);
                enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
                enqueueCommand(CMD_REQUEST_DEBUG_LINES);
                //enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
            }
        }
    }


}
Beispiel #8
0
void	PhysicsClient::createClientCommand()
{
	if (!m_waitingForServer)
		{
			//process outstanding requests
			if (m_userCommandRequests.size())
			{
				b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
				int command = m_userCommandRequests[0];
				
				//don't use 'remove' because it will re-order the commands
				//m_userCommandRequests.remove(command);
				//pop_front
				for (int i=1;i<m_userCommandRequests.size();i++)
				{
					m_userCommandRequests[i-1] = m_userCommandRequests[i];
				}

				m_userCommandRequests.pop_back();
				m_waitingForServer = true;

				switch (command)
				{
				case CMD_LOAD_URDF:
					{
						if (!m_serverLoadUrdfOK)
						{
							m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
							sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;

							m_testBlock1->m_numClientCommands++;
							b3Printf("Client created CMD_LOAD_URDF\n");
						} else
						{
							b3Warning("Server already loaded URDF, no client command submitted\n");
						}
						break;
					}
				case CMD_CREATE_BOX_COLLISION_SHAPE:
					{
						if (m_serverLoadUrdfOK)
						{
							b3Printf("Requesting create box collision shape\n");
							m_testBlock1->m_clientCommands[0].m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
							m_testBlock1->m_numClientCommands++;
						} else
						{
							b3Warning("No URDF loaded\n");
						}
						break;
					}
				case CMD_SEND_BULLET_DATA_STREAM:
					{
						b3Printf("Sending a Bullet Data Stream\n");
						///The idea is to pass a stream of chunks from client to server
						///over shared memory. The server will process it
						///Initially we will just copy an entire .bullet file into shared
						///memory but we can also send individual chunks one at a time
						///so it becomes a streaming solution
						///In addition, we can make a separate API to create those chunks
						///if needed, instead of using a 3d modeler or the Bullet SDK btSerializer
						
						char relativeFileName[1024];
						const char* fileName = "slope.bullet";
						bool fileFound = b3ResourcePath::findResourcePath(fileName,relativeFileName,1024);
						if (fileFound)
						{
							FILE *fp = fopen(relativeFileName, "rb");
							if (fp)
							{
								fseek(fp, 0L, SEEK_END);
								int mFileLen = ftell(fp);
								fseek(fp, 0L, SEEK_SET);
								if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
								{
								
									fread(m_testBlock1->m_bulletStreamDataClientToServer, mFileLen, 1, fp);

									fclose(fp);

									m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_BULLET_DATA_STREAM;
									m_testBlock1->m_clientCommands[0].m_dataStreamArguments.m_streamChunkLength = mFileLen;
									m_testBlock1->m_numClientCommands++;
									b3Printf("Send bullet data stream command\n");
								} else
								{
									b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
								}
							} else
							{
								b3Warning("Cannot open file %s\n", relativeFileName);
							}
						} else
						{
							b3Warning("Cannot find file %s\n", fileName);
						}
						
						break;
					}
				case CMD_REQUEST_ACTUAL_STATE:
					{
						if (m_serverLoadUrdfOK)
						{
							b3Printf("Requesting actual state\n");
							m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
							m_testBlock1->m_numClientCommands++;

						} else
						{
							b3Warning("No URDF loaded\n");
						}
						break;
					}
				case CMD_STEP_FORWARD_SIMULATION:
					{
						if (m_serverLoadUrdfOK)
						{
						
							m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
							m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
							m_testBlock1->m_numClientCommands++;
							b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++);
						} else
						{
							b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n");
						}
						break;
					}
				case CMD_SHUTDOWN:
					{
						m_wantsTermination = true;
						m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
						m_testBlock1->m_numClientCommands++;
						m_serverLoadUrdfOK = false;
						b3Printf("client created CMD_SHUTDOWN\n");
						break;
					}
				default:
					{
						b3Error("unknown command requested\n");
					}
				}
			}
		}

}
void	PhysicsClient::createClientCommand()
{
	if (!m_waitingForServer)
		{
			//process outstanding requests
			if (m_userCommandRequests.size())
			{
				b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
				int command = m_userCommandRequests[0];
				
				//don't use 'remove' because it will re-order the commands
				//m_userCommandRequests.remove(command);
				//pop_front
				for (int i=1;i<m_userCommandRequests.size();i++)
				{
					m_userCommandRequests[i-1] = m_userCommandRequests[i];
				}

				m_userCommandRequests.pop_back();
				m_waitingForServer = true;

				switch (command)
				{
				case CMD_LOAD_URDF:
					{
						if (!m_serverLoadUrdfOK)
						{
							m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
							sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
							m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;

							m_testBlock1->m_numClientCommands++;
							b3Printf("Client created CMD_LOAD_URDF\n");
						} else
						{
							b3Warning("Server already loaded URDF, no client command submitted\n");
						}
						break;
					}
				case CMD_REQUEST_ACTUAL_STATE:
					{
						if (m_serverLoadUrdfOK)
						{
							b3Printf("Requesting actual state\n");
							m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
							m_testBlock1->m_numClientCommands++;

						} else
						{
							b3Warning("No URDF loaded\n");
						}
						break;
					}
				case CMD_STEP_FORWARD_SIMULATION:
					{
						if (m_serverLoadUrdfOK)
						{
						
							m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
							m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
							m_testBlock1->m_numClientCommands++;
							b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++);
						} else
						{
							b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n");
						}
						break;
					}
				case CMD_SHUTDOWN:
					{
						m_wantsTermination = true;
						m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
						m_testBlock1->m_numClientCommands++;
						m_serverLoadUrdfOK = false;
						b3Printf("client created CMD_SHUTDOWN\n");
						break;
					}
				default:
					{
						b3Error("unknown command requested\n");
					}
				}
			}
		}

}