Beispiel #1
0
static PyObject *
pybullet_loadURDF(PyObject* self, PyObject* args)
{
    
	int size= PySequence_Size(args);
	
	int bodyIndex = -1;
	const char* urdfFileName="";
	float startPosX =0;
    float startPosY =0;
    float startPosZ = 1;
	float startOrnX = 0;
	float startOrnY = 0;
	float startOrnZ = 0;
	float startOrnW = 1;
	//printf("size=%d\n", size);
	if (0==sm)
    {
        PyErr_SetString(SpamError, "Not connected to physics server.");
        return NULL;
    }
	if (size==1)
	{
		if (!PyArg_ParseTuple(args, "s", &urdfFileName))
       	 	return NULL;            
	}
	if (size == 4)
	{
	  if (!PyArg_ParseTuple(args, "sfff", &urdfFileName,
		&startPosX,&startPosY,&startPosZ))
                return NULL;
        }
	if (size==8)
	{
		if (!PyArg_ParseTuple(args, "sfffffff", &urdfFileName,
		&startPosX,&startPosY,&startPosZ,
			&startOrnX,&startOrnY,&startOrnZ, &startOrnW))
                return NULL;
		
	}
    {
        
        b3SharedMemoryStatusHandle statusHandle;
        int statusType;
        b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
		//printf("urdf filename = %s\n", urdfFileName);
        //setting the initial position, orientation and other arguments are optional
        b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
        statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
        statusType = b3GetStatusType(statusHandle);
        if (statusType!=CMD_URDF_LOADING_COMPLETED)
		{
			PyErr_SetString(SpamError, "Cannot load URDF file.");
			return NULL;
		}
        bodyIndex = b3GetStatusBodyIndex(statusHandle);
    }
	return PyLong_FromLong(bodyIndex);
}
Beispiel #2
0
int main(int argc, char* argv[])
{
	kPhysClient = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
	if (!kPhysClient)
		return -1;
	// visualizer
	command = b3InitConfigureOpenGLVisualizer(kPhysClient);
	b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_GUI, 0);
	b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
	b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_SHADOWS, 0);
	b3SubmitClientCommandAndWaitStatus(kPhysClient, command);

	b3SetTimeOut(kPhysClient, 10);

	//syncBodies is only needed when connecting to an existing physics server that has already some bodies
	command = b3InitSyncBodyInfoCommand(kPhysClient);
	statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
	statusType = b3GetStatusType(statusHandle);

	// set fixed time step
	command = b3InitPhysicsParamCommand(kPhysClient);
	ret = b3PhysicsParamSetTimeStep(command, FIXED_TIMESTEP);
	statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
	ret = b3PhysicsParamSetRealTimeSimulation(command, false);
	statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);

	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);

	// load test
	command = b3LoadUrdfCommandInit(kPhysClient, "TwoJointRobot_wo_fixedJoints.urdf");
	int flags = URDF_USE_INERTIA_FROM_FILE;
	b3LoadUrdfCommandSetFlags(command, flags);
	b3LoadUrdfCommandSetUseFixedBase(command, true);
	// q.setEulerZYX(0, 0, 0);
	// b3LoadUrdfCommandSetStartOrientation(command, q[0], q[1], q[2], q[3]);
	b3LoadUrdfCommandSetUseMultiBody(command, true);
	statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
	statusType = b3GetStatusType(statusHandle);
	b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
	if (statusType == CMD_URDF_LOADING_COMPLETED)
	{
		twojoint = b3GetStatusBodyIndex(statusHandle);
	}

	//disable default linear/angular damping
	b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(kPhysClient);
	double linearDamping = 0;
	double angularDamping = 0;
	b3ChangeDynamicsInfoSetLinearDamping(command, twojoint, linearDamping);
	b3ChangeDynamicsInfoSetAngularDamping(command, twojoint, angularDamping);
	statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);

	int numJoints = b3GetNumJoints(kPhysClient, twojoint);
	printf("twojoint numjoints = %d\n", numJoints);
	// Loop through all joints
	for (int i = 0; i < numJoints; ++i)
	{
		b3GetJointInfo(kPhysClient, twojoint, i, &jointInfo);
		if (jointInfo.m_jointName[0])
		{
			jointNameToId[std::string(jointInfo.m_jointName)] = i;
		}
		else
		{
			continue;
		}
		// Reset before torque control - see #1459
		command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);
		b3JointControlSetDesiredVelocity(command, jointInfo.m_uIndex, 0);
		b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);
		statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
	}

	// loop
	unsigned long dtus1 = (unsigned long)1000000.0 * FIXED_TIMESTEP;
	double simTimeS = 0;
	double q[2], v[2];
	while (b3CanSubmitCommand(kPhysClient))
	{
		simTimeS += 0.000001 * dtus1;
		// apply some torque
		b3GetJointInfo(kPhysClient, twojoint, jointNameToId["joint_2"], &jointInfo);
		command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE);
		b3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 0.5 * sin(10 * simTimeS));
		statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);

		// get joint values
		command = b3RequestActualStateCommandInit(kPhysClient, twojoint);
		statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
		b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_1"], &state);
		q[0] = state.m_jointPosition;
		v[0] = state.m_jointVelocity;
		b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_2"], &state);
		q[1] = state.m_jointPosition;
		v[1] = state.m_jointVelocity;

		statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));

		// debugging output
		printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", simTimeS, q[0], q[1], v[0], v[1]);
		b3Clock::usleep(1000. * 1000. * FIXED_TIMESTEP);
	}
	b3DisconnectSharedMemory(kPhysClient);
	return 0;
}
Beispiel #3
0
int main(int argc, char* argv[])
{
	int i, dofCount , posVarCount, dofIndex, ret ,numJoints, allowSharedMemoryInitialization=0;
    int sensorJointIndexLeft=-1;
    int sensorJointIndexRight=-1;
    int statusType = -1;
	const char* urdfFileName = "r2d2.urdf";
	double gravx=0, gravy=0, gravz=-9.8;
	double timeStep = 1./60.;
	double startPosX, startPosY,startPosZ;
	int imuLinkIndex = -1;

	
	b3PhysicsClientHandle sm=0;
	int bodyIndex = -1;

	printf("hello world\n");
#ifdef PHYSICS_LOOP_BACK
	sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
#else
	sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
#endif
	
	

	if (b3CanSubmitCommand(sm))
	{
        {
        b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
		ret = b3PhysicsParamSetGravity(command,  gravx,gravy, gravz);
		ret = b3PhysicsParamSetTimeStep(command,  timeStep);
		b3SubmitClientCommandAndWaitStatus(sm, command);
        }

		
        {
            b3SharedMemoryStatusHandle statusHandle;
			b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
			
            //setting the initial position, orientation and other arguments are optional
            startPosX =2;
            startPosY =3;
            startPosZ = 1;
            ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
			bodyIndex = b3GetStatusBodyIndex(statusHandle);
        }
        
		if (bodyIndex>=0)
		{
			numJoints = b3GetNumJoints(sm,bodyIndex);
			for (i=0;i<numJoints;i++)
			{
				struct b3JointInfo jointInfo;
				b3GetJointInfo(sm,bodyIndex, i,&jointInfo);
            
				printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
				//pick the IMU link index based on torso name
				if (strstr(jointInfo.m_linkName,"base_link"))
				{
					imuLinkIndex = i;
				}
            
				//pick the joint index based on joint name
				if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
				{
					sensorJointIndexLeft = i;
				}
				if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
				{
					sensorJointIndexRight = i;
				}
            
			}
        
			if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
			{
				b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
				b3SharedMemoryStatusHandle statusHandle;
				if (imuLinkIndex>=0)
				{
					 ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
				}
            
				if (sensorJointIndexLeft>=0)
				{
				  ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
				}
				if(sensorJointIndexRight>=0)
				{
					ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
				}
				statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
            
			}
		}
        
        {
            b3SharedMemoryStatusHandle statusHandle;
             b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm);
            ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1);
            ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1);
            ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1);
            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);

        }

        {
        		int statusType;
            b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex);
            b3SharedMemoryStatusHandle statusHandle;
            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
            statusType = b3GetStatusType(statusHandle);
            
            if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
            {
                b3GetStatusActualState(statusHandle,
                                       0, &posVarCount, &dofCount,
                                       0, 0, 0, 0);

                b3Printf("posVarCount = %d\n",posVarCount);
                printf("dofCount = %d\n",dofCount);
            }
        }
        
        {
#if 0
            b3SharedMemoryStatusHandle statusHandle;
             b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
            for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
            {
                b3JointControlSetDesiredVelocity(command,dofIndex,1);
                b3JointControlSetMaximumForce(command,dofIndex,100);
            }
             statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
#endif
        }
        ///perform some simulation steps for testing
        for ( i=0;i<100;i++)
        {
            b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
        }
        
        {
            b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex));
        
			if (sensorJointIndexLeft>=0)
			{

				struct  b3JointSensorState sensorState;
				b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState);
				
				b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
					sensorState.m_jointForceTorque[0],
					sensorState.m_jointForceTorque[1],
					sensorState.m_jointForceTorque[2]);

			}
        
			if (sensorJointIndexRight>=0)
			{
				struct  b3JointSensorState sensorState;
				b3GetJointState(sm,state,sensorJointIndexRight,&sensorState);
				
				b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
						 sensorState.m_jointForceTorque[0],
						 sensorState.m_jointForceTorque[1],
						 sensorState.m_jointForceTorque[2]);

			}
		}
        

        {
            b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
        }
        
	}


	b3DisconnectSharedMemory(sm);
}
Beispiel #4
0
void testSharedMemory(b3PhysicsClientHandle sm)
{
	int i, dofCount , posVarCount, ret ,numJoints ;
    int sensorJointIndexLeft=-1;
    int sensorJointIndexRight=-1;
	const char* urdfFileName = "r2d2.urdf";
	const char* sdfFileName = "kuka_iiwa/model.sdf";
	double gravx=0, gravy=0, gravz=-9.8;
	double timeStep = 1./60.;
	double startPosX, startPosY,startPosZ;
	int imuLinkIndex = -1;
	int bodyIndex = -1;

	if (b3CanSubmitCommand(sm))
	{
        {
        b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
        b3SharedMemoryStatusHandle statusHandle;
		ret = b3PhysicsParamSetGravity(command,  gravx,gravy, gravz);
		ret = b3PhysicsParamSetTimeStep(command,  timeStep);
		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
        ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
        }

        {
            b3SharedMemoryStatusHandle statusHandle;
            int statusType;
            int bodyIndicesOut[10];//MAX_SDF_BODIES = 10
            int numJoints, numBodies;
			int bodyUniqueId;
            b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
			statusType = b3GetStatusType(statusHandle);
			ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED);
			
			numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
            ASSERT_EQ(numBodies,1);
            bodyUniqueId = bodyIndicesOut[0];
			{
				{
					b3SharedMemoryStatusHandle statusHandle;
					int statusType;
					b3SharedMemoryCommandHandle command = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
					statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
					statusType = b3GetStatusType(statusHandle);
					if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
					{
						struct b3VisualShapeInformation vi;
						b3GetVisualShapeInformation(sm, &vi);
					}
				}
			}
            
            numJoints = b3GetNumJoints(sm,bodyUniqueId);
            ASSERT_EQ(numJoints,7);

#if 0
            b3Printf("numJoints: %d\n", numJoints);
            for (i=0;i<numJoints;i++)
            {
                struct b3JointInfo jointInfo;
                if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo))
                {
                    b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
                }
            }
#endif
            {
                b3SharedMemoryStatusHandle statusHandle;
                b3SharedMemoryCommandHandle commandHandle;
                double jointAngle = 0.f;
               	int jointIndex; 
                commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
                for (jointIndex=0;jointIndex<numJoints;jointIndex++)
                {
                    b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle);
                }
                
                statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);

                ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
            }
            
        }
        
        
        {
            b3SharedMemoryStatusHandle statusHandle;
			int statusType;
			b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
			
            //setting the initial position, orientation and other arguments are optional
            startPosX =2;
            startPosY =0;
            startPosZ = 1;
            ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
			statusType = b3GetStatusType(statusHandle);
			ASSERT_EQ(statusType, CMD_URDF_LOADING_COMPLETED);
			bodyIndex = b3GetStatusBodyIndex(statusHandle);
        }
        
		if (bodyIndex>=0)
		{
			

			numJoints = b3GetNumJoints(sm,bodyIndex);
			for (i=0;i<numJoints;i++)
			{
				struct b3JointInfo jointInfo;
				b3GetJointInfo(sm,bodyIndex, i,&jointInfo);
            
			//	printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
				//pick the IMU link index based on torso name
				if (strstr(jointInfo.m_linkName,"base_link"))
				{
					imuLinkIndex = i;
				}
            
				//pick the joint index based on joint name
				if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
				{
					sensorJointIndexLeft = i;
				}
				if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
				{
					sensorJointIndexRight = i;
				}
            
			}
        
			if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
			{
				b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex);
				b3SharedMemoryStatusHandle statusHandle;
				if (imuLinkIndex>=0)
				{
					 ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
				}
            
				if (sensorJointIndexLeft>=0)
				{
				  ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
				}
				if(sensorJointIndexRight>=0)
				{
					ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
				}
				statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
                ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
			}
		}
        
        {
            b3SharedMemoryStatusHandle statusHandle;
             b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm);
            ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1);
            ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1);
            ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1);
            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
            ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RIGID_BODY_CREATION_COMPLETED);

        }

        {
        		int statusType;
            b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex);
            b3SharedMemoryStatusHandle statusHandle;
            statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
            statusType = b3GetStatusType(statusHandle);
           ASSERT_EQ(statusType, CMD_ACTUAL_STATE_UPDATE_COMPLETED);
 
            if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
            {
                b3GetStatusActualState(statusHandle,
                                       0, &posVarCount, &dofCount,
                                       0, 0, 0, 0);
                ASSERT_EQ(posVarCount,15);
                ASSERT_EQ(dofCount,14);
            }
        }
        
        {
#if 0
            b3SharedMemoryStatusHandle statusHandle;
             b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
            for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
            {
                b3JointControlSetDesiredVelocity(command,dofIndex,1);
                b3JointControlSetMaximumForce(command,dofIndex,100);
            }
             statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
#endif
        }
        ///perform some simulation steps for testing
        for ( i=0;i<100;i++)
        {
			b3SharedMemoryStatusHandle statusHandle;
			int statusType;

            if (b3CanSubmitCommand(sm))
            {
                statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
                statusType = b3GetStatusType(statusHandle);
                ASSERT_EQ(statusType, CMD_STEP_FORWARD_SIMULATION_COMPLETED);
            } else
            {
                break;
            }
        }
        
        if (b3CanSubmitCommand(sm))
        {
            b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex));
        
			if (sensorJointIndexLeft>=0)
			{

				struct  b3JointSensorState sensorState;
				b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState);
				
				b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
					sensorState.m_jointForceTorque[0],
					sensorState.m_jointForceTorque[1],
					sensorState.m_jointForceTorque[2]);

			}
        
			if (sensorJointIndexRight>=0)
			{
				struct  b3JointSensorState sensorState;
				b3GetJointState(sm,state,sensorJointIndexRight,&sensorState);
				
				b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
						 sensorState.m_jointForceTorque[0],
						 sensorState.m_jointForceTorque[1],
						 sensorState.m_jointForceTorque[2]);

			}
        

            {
                b3SharedMemoryStatusHandle statusHandle;
                statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
                ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED);
            }
        }
        
	}


	b3DisconnectSharedMemory(sm);
}
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);
            }
        }
    }


}