Ejemplo n.º 1
0
int main(int argc, char* argv[])
{
#ifdef PHYSICS_LOOP_BACK
        b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
#endif

#ifdef PHYSICS_SERVER_DIRECT
        b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
#endif

#ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER

#ifdef __APPLE__
    b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc,argv);
#else
    b3PhysicsClientHandle sm = b3CreateInProcessPhysicsServerAndConnect(argc,argv);
#endif //__APPLE__
#endif

#ifdef PHYSICS_SHARED_MEMORY
        b3PhysicsClientHandle sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
#endif //PHYSICS_SHARED_MEMORY

#ifdef PHYSICS_UDP
        b3PhysicsClientHandle sm = b3ConnectPhysicsUDP("localhost",1234);
#endif //PHYSICS_UDP

	testSharedMemory(sm);
}
Ejemplo n.º 2
0
static PyObject *
pybullet_connectPhysicsServer(PyObject *self, PyObject *args)
{
    if (0!=sm)
    {
        PyErr_SetString(SpamError, "Already connected to physics server, disconnect first.");
        return NULL;
    }
    
    {
		int method=eCONNECT_GUI;
		if (!PyArg_ParseTuple(args, "i", &method))
		{
			PyErr_SetString(SpamError, "connectPhysicsServer expected argument  eCONNECT_GUI, eCONNECT_DIRECT or eCONNECT_SHARED_MEMORY");
	        return NULL;
		}

		switch (method)
		{
			case eCONNECT_GUI:
			{
				int argc=0;
				char* argv[1]={0};

#ifdef __APPLE__
				sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
#else
				sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
#endif
				break;
			}
			case eCONNECT_DIRECT:
			{
				sm = b3ConnectPhysicsDirect();
				break;
			}
			case eCONNECT_SHARED_MEMORY:
			{
				sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
				break;
			}


			default:
				{
					PyErr_SetString(SpamError, "connectPhysicsServer unexpected argument");
					return NULL;
				}
		};
        
	
    }
    
	Py_INCREF(Py_None);
    return Py_None;
}
Ejemplo n.º 3
0
TEST(BulletPhysicsClientServerTest, DirectConnection) {
        b3PhysicsClientHandle sm = b3ConnectPhysicsDirect();
        testSharedMemory(sm);
}
Ejemplo n.º 4
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);
#endif

#ifdef PHYSICS_SERVER_DIRECT
	sm = b3ConnectPhysicsDirect();
#else//PHYSICS_SERVER_DIRECT
	sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
#endif //PHYSICS_SERVER_DIRECT
	
	

	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);
}
Ejemplo n.º 5
0
void	PhysicsClientExample::initPhysics()
{
	if (m_guiHelper && m_guiHelper->getParameterInterface())
	{
		int upAxis = 2;
		m_guiHelper->setUpAxis(upAxis);

		createButtons();		
		
	} else
	{
        MyCallback(CMD_LOAD_URDF, true, this);
        MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
        MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
        MyCallback(CMD_RESET_SIMULATION,true,this);
	}

	m_selectedBody = -1;
	m_prevSelectedBody = -1;

	{
		m_canvas = m_guiHelper->get2dCanvasInterface();
		if (m_canvas)
		{
			

			m_canvasIndex = m_canvas->createCanvas("Synthetic Camera",camVisualizerWidth, camVisualizerHeight);

			for (int i=0;i<camVisualizerWidth;i++)
			{
				for (int j=0;j<camVisualizerHeight;j++)
				{
					unsigned char red=255;
					unsigned char green=255;
					unsigned char blue=255;
					unsigned char alpha=255;
					if (i==j)
					{
						red = 0;
						green=0;
						blue=0;
					}
					m_canvas->setPixel(m_canvasIndex,i,j,red,green,blue,alpha);
				}
			}
			m_canvas->refreshImageData(m_canvasIndex);
			
		}

	}

    if (m_options == eCLIENTEXAMPLE_SERVER)
    {
        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);
	}
	
    if (!b3CanSubmitCommand(m_physicsClientHandle))
    {
		b3Warning("Cannot connect to physics client");
	}

}