コード例 #1
0
ファイル: pybullet.c プロジェクト: bertleft/bullet3
static PyObject *
pybullet_setGravity(PyObject* self, PyObject* args)
{
    if (0==sm)
    {
        PyErr_SetString(SpamError, "Not connected to physics server.");
        return NULL;
    }

	{
		float gravX=0;
		float gravY=0;
		float gravZ=-10;
		int ret;

		b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
		b3SharedMemoryStatusHandle statusHandle;

		if (!PyArg_ParseTuple(args, "fff", &gravX,&gravY,&gravZ))
		{
			PyErr_SetString(SpamError, "setGravity expected (x,y,z) values.");
			return NULL;
		}
		ret = b3PhysicsParamSetGravity(command,  gravX,gravY, gravZ);
		//ret = b3PhysicsParamSetTimeStep(command,  timeStep);
		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
		//ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
	}

	Py_INCREF(Py_None);
	return Py_None;
}
コード例 #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;
}
コード例 #3
0
ファイル: test.c プロジェクト: 20-sim/bullet3
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);
}
コード例 #4
0
ファイル: test.c プロジェクト: WhitestormJS/ammo.js
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);
}
コード例 #5
0
void MyCallback2(int buttonId, bool buttonState, void* userPtr)
{
	RobotControlExample* cl = (RobotControlExample*) userPtr;

	SharedMemoryCommand command;

	switch (buttonId)
	{
	case  CMD_LOAD_URDF:
		{
			command.m_type =CMD_LOAD_URDF;
			command.m_updateFlags = URDF_ARGS_FILE_NAME|URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION;
			sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf");//r2d2.urdf");
			command.m_urdfArguments.m_initialPosition[0] = 0.0;
			command.m_urdfArguments.m_initialPosition[1] = 0.0;
			command.m_urdfArguments.m_initialPosition[2] = 0.0;
			command.m_urdfArguments.m_initialOrientation[0] = 0.0;
			command.m_urdfArguments.m_initialOrientation[1] = 0.0;
			command.m_urdfArguments.m_initialOrientation[2] = 0.0;
			command.m_urdfArguments.m_initialOrientation[3] = 1.0;
			command.m_urdfArguments.m_useFixedBase = false;
			command.m_urdfArguments.m_useMultiBody = true;
			cl->enqueueCommand(command);
			break;
		}

		case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
		{
		    //#ifdef USE_C_API
		    b3InitPhysicsParamCommand(&command);
            b3PhysicsParamSetGravity(&command, 1,1,-10);
            

//		    #else
//		    
//			command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
//			command.m_physSimParamArgs.m_gravityAcceleration[0] = 0;
//			command.m_physSimParamArgs.m_gravityAcceleration[1] = 0;
//			command.m_physSimParamArgs.m_gravityAcceleration[2] = -10;
//			command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY;
//			#endif // USE_C_API
			
			cl->enqueueCommand(command);
			break;

		};
		case CMD_INIT_POSE:
		{
			///@todo: implement this
			command.m_type = CMD_INIT_POSE;
			cl->enqueueCommand(command);
			break;
		}

			
	case CMD_CREATE_BOX_COLLISION_SHAPE:
		{
			command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
            command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
            command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
            command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
            command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
            
            
			cl->enqueueCommand(command);
			break;
		}
	case CMD_REQUEST_ACTUAL_STATE:
		{
			command.m_type =CMD_REQUEST_ACTUAL_STATE;
			cl->enqueueCommand(command);
			break;
		};
	case CMD_STEP_FORWARD_SIMULATION:
		{
			command.m_type =CMD_STEP_FORWARD_SIMULATION;
			cl->enqueueCommand(command);
			break;
		}
	
    case CMD_SEND_DESIRED_STATE:
		{
			
			command.m_type =CMD_SEND_DESIRED_STATE;
			cl->prepareControlCommand(command);
			cl->enqueueCommand(command);		
			break;
		}
	case CMD_SEND_BULLET_DATA_STREAM:
		{
			command.m_type = buttonId;
			sprintf(command.m_dataStreamArguments.m_bulletFileName,"slope.bullet");
			command.m_dataStreamArguments.m_streamChunkLength = 0;
			cl->enqueueCommand(command);
			break;
		}

	default:
		{
			b3Error("Unknown buttonId");
			btAssert(0);
		}
	};
}
コード例 #6
0
void MyCallback2(int buttonId, bool buttonState, void* userPtr)
{
	RobotControlExample* cl = (RobotControlExample*) userPtr;

	SharedMemoryCommand command;

	switch (buttonId)
	{
	case  CMD_LOAD_URDF:
		{
			command.m_type =CMD_LOAD_URDF;
			sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");
			command.m_urdfArguments.m_initialPosition[0] = 0.0;
			command.m_urdfArguments.m_initialPosition[1] = 0.0;
			command.m_urdfArguments.m_initialPosition[2] = 0.0;
			command.m_urdfArguments.m_initialOrientation[0] = 0.0;
			command.m_urdfArguments.m_initialOrientation[1] = 0.0;
			command.m_urdfArguments.m_initialOrientation[2] = 0.0;
			command.m_urdfArguments.m_initialOrientation[3] = 1.0;
			command.m_urdfArguments.m_useFixedBase = false;
			command.m_urdfArguments.m_useMultiBody = true;
			cl->enqueueCommand(command);
			break;
		}

		case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
		{
		    //#ifdef USE_C_API
		    b3InitPhysicsParamCommand(&command);
            b3PhysicsParamSetGravity(&command, 0,0,-10);
            

//		    #else
//		    
//			command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
//			command.m_physSimParamArgs.m_gravityAcceleration[0] = 0;
//			command.m_physSimParamArgs.m_gravityAcceleration[1] = 0;
//			command.m_physSimParamArgs.m_gravityAcceleration[2] = -10;
//			command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY;
//			#endif // USE_C_API
			
			cl->enqueueCommand(command);
			break;

		};
		case CMD_INIT_POSE:
		{
			///@todo: implement this
			command.m_type = CMD_INIT_POSE;
			cl->enqueueCommand(command);
			break;
		}

			
	case CMD_CREATE_BOX_COLLISION_SHAPE:
		{
			command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
			cl->enqueueCommand(command);
			break;
		}
	case CMD_REQUEST_ACTUAL_STATE:
		{
			command.m_type =CMD_REQUEST_ACTUAL_STATE;
			cl->enqueueCommand(command);
			break;
		};
	case CMD_STEP_FORWARD_SIMULATION:
		{
			command.m_type =CMD_STEP_FORWARD_SIMULATION;
			cl->enqueueCommand(command);
			break;
		}
	
    case CMD_SEND_DESIRED_STATE:
		{
			
				command.m_type =CMD_SEND_DESIRED_STATE;
				int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;

				command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
				//todo: expose a drop box in the GUI for this
				switch (controlMode)
				{
				case CONTROL_MODE_VELOCITY:
					{
						for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
						{
							command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
							command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
						}
						for (int i=0;i<cl->m_numMotors;i++)
						{
							btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget;
                            
							int uIndex = cl->m_motorTargetVelocities[i].m_uIndex;
							if (targetVel>1)
                            {
                                printf("testme");
                            }
                            command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
							
						}
						break;
					}
				case CONTROL_MODE_TORQUE:
					{
						for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
						{
							command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
						}
						break;
					}
				default:
					{
						b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
						btAssert(0);
					}
				}
			cl->enqueueCommand(command);		
			break;
		}
	case CMD_SEND_BULLET_DATA_STREAM:
		{
			command.m_type = buttonId;
			cl->enqueueCommand(command);
			break;
		}

	default:
		{
			b3Error("Unknown buttonId");
			btAssert(0);
		}
	};
}
コード例 #7
0
void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
    
    switch (commandId)
    {
        case  CMD_LOAD_URDF:
        {
            
            b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
            
            //setting the initial position, orientation and other arguments are optional
            double startPosX = 0;
            static double startPosY = 0;
            double startPosZ = 0;
            b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ);
			startPosY += 2.f;
//            ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);

            break;
        }
        case CMD_REQUEST_CAMERA_IMAGE_DATA:
        {
            ///request an image from a simulated camera, using a software renderer.
            
            b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
            
			float viewMatrix[16];
			float projectionMatrix[16];
			this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
			this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);
            b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        case CMD_CREATE_BOX_COLLISION_SHAPE:
        {
            b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
            b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3);
			b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
		case CMD_CREATE_RIGID_BODY:
		{
			b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
            b3CreateBoxCommandSetStartPosition(commandHandle,0,0,0);
			b3CreateBoxCommandSetMass(commandHandle,1);
			b3CreateBoxCommandSetCollisionShapeType(commandHandle,COLLISION_SHAPE_TYPE_CYLINDER_Y);
			b3CreateBoxCommandSetColorRGBA(commandHandle,1,1,0,1);
			double radius = 0.2;
			double halfHeight = 0.5;
			b3CreateBoxCommandSetHalfExtents(commandHandle,radius,halfHeight,radius);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

        case CMD_REQUEST_ACTUAL_STATE:
        {
			if (m_selectedBody>=0)
			{
				b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody);
                b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
                
                int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
                for (int i = 0; i < numJoints; ++i) {
                    struct b3JointSensorState sensorState;
                    b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
                    b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
                }
			}
            break;
        };

		case CMD_INIT_POSE:
		{
			
			if (m_selectedBody>=0)
			{
				b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle,m_selectedBody);
				static int toggle = 0;
				double pos[3] = {0,0,0};
				pos[toggle] = 2;
				toggle++; 
				if (toggle>2)
					toggle=0;

				btQuaternion orn;
				orn.setValue(0,0,0,1);

				switch (toggle)
				{
				case 0:
					orn = btQuaternion(btVector3(1,0,0),SIMD_HALF_PI);
					break;
				case 1:
					orn = btQuaternion(btVector3(0,1,0),SIMD_HALF_PI);
					break;
				case 2:
					orn = btQuaternion(btVector3(0,0,1),SIMD_HALF_PI);
					break;

				default:
					orn.setValue(0,0,0,1);
				};
				

				b3CreatePoseCommandSetBaseOrientation(commandHandle,orn[0],orn[1],orn[2],orn[3]);
				b3CreatePoseCommandSetBasePosition(commandHandle, pos[0],pos[1],pos[2]);
				int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
				static double jointPos = SIMD_PI/2.f;
				
				for (int i=0;i<numJoints;i++)
				{
					b3JointInfo info;
					b3GetJointInfo(m_physicsClientHandle, m_selectedBody,i, &info);
					if ((info.m_jointType == 0) || (info.m_jointType == 1)) //revolute or prismatic
					{
						b3CreatePoseCommandSetJointPosition(m_physicsClientHandle,commandHandle,i,jointPos);
					}
				}
				jointPos += SIMD_PI/8.0;
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			}
			break;
		}
        case CMD_STEP_FORWARD_SIMULATION:
        {
        
            b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
            
        case CMD_REQUEST_DEBUG_LINES:
        {
            b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        case CMD_SEND_DESIRED_STATE:
        {
            // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY);
            b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD);
            prepareControlCommand(command);
            b3SubmitClientCommand(m_physicsClientHandle, command);
            break;
        }
        case CMD_RESET_SIMULATION:
        {
            b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        case CMD_SEND_BULLET_DATA_STREAM:
        {
#if 0

            //this worked, but needs C-API and a streaming options, similar to debug lines
            command.m_type = buttonId;
            cl->enqueueCommand(command);
#endif

            break;
        }
        case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: {
            b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
            b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
            b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
            break;
        }
        default:
        {
            b3Error("Unknown buttonId");
            btAssert(0);
        }
    };
}
コード例 #8
0
void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
{
	switch (commandId)
	{
		case CMD_LOAD_URDF:
		{
			b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
			//setting the initial position, orientation and other arguments are optional
			double startPosX = 0;
			static double startPosY = 0;
			double startPosZ = 0;
			b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX, startPosY, startPosZ);
			startPosY += 2.f;
			//            ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_LOAD_SDF:
		{
#ifdef BT_DEBUG
			b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");
#else
			b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kitchens/1.sdf");  //two_cubes.sdf");//kitchens/1.sdf");//kuka_iiwa/model.sdf");
#endif
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_REQUEST_CAMERA_IMAGE_DATA:
		{
			///request an image from a simulated camera, using a software renderer.
			b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
			//b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL);

			float viewMatrix[16];
			float projectionMatrix[16];
			m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
			m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);

			b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix);
			b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight);
			float lightPos[3];
			lightPos[0] = m_lightPos[0];
			lightPos[1] = m_lightPos[1];
			lightPos[2] = m_lightPos[2];
			b3RequestCameraImageSetLightDirection(commandHandle, lightPos);
			b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_CREATE_BOX_COLLISION_SHAPE:
		{
			b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
			b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, -1.5);
			b3CreateBoxCommandSetColorRGBA(commandHandle, 0, 0, 1, 1);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_CREATE_RIGID_BODY:
		{
			b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle);
			b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, 0);
			b3CreateBoxCommandSetMass(commandHandle, 1);
			b3CreateBoxCommandSetCollisionShapeType(commandHandle, COLLISION_SHAPE_TYPE_CYLINDER_Y);
			b3CreateBoxCommandSetColorRGBA(commandHandle, 1, 1, 0, 1);
			double radius = 0.2;
			double halfHeight = 0.5;
			b3CreateBoxCommandSetHalfExtents(commandHandle, radius, halfHeight, radius);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_REQUEST_ACTUAL_STATE:
		{
			if (m_selectedBody >= 0)
			{
				b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle, m_selectedBody);
				b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle);
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);

				int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
				for (int i = 0; i < numJoints; ++i)
				{
					struct b3JointSensorState sensorState;
					b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState);
					//b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque);
				}
			}

			break;
		};

		case CMD_INIT_POSE:
		{
			if (m_selectedBody >= 0)
			{
				b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle, m_selectedBody);
				static int toggle = 0;
				double pos[3] = {0, 0, 0};
				pos[toggle] = 2;
				toggle++;
				if (toggle > 2)
					toggle = 0;

				btQuaternion orn;
				orn.setValue(0, 0, 0, 1);

				switch (toggle)
				{
					case 0:
						orn = btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI);
						break;
					case 1:
						orn = btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
						break;
					case 2:
						orn = btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI);
						break;

					default:
						orn.setValue(0, 0, 0, 1);
				};

				b3CreatePoseCommandSetBaseOrientation(commandHandle, orn[0], orn[1], orn[2], orn[3]);
				b3CreatePoseCommandSetBasePosition(commandHandle, pos[0], pos[1], pos[2]);
				int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
				static double jointPos = SIMD_PI / 2.f;

				for (int i = 0; i < numJoints; i++)
				{
					b3JointInfo info;
					b3GetJointInfo(m_physicsClientHandle, m_selectedBody, i, &info);
					if ((info.m_jointType == 0) || (info.m_jointType == 1))  //revolute or prismatic
					{
						b3CreatePoseCommandSetJointPosition(m_physicsClientHandle, commandHandle, i, jointPos);
					}
				}
				jointPos += SIMD_PI / 8.0;
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			}
			break;
		}
		case CMD_STEP_FORWARD_SIMULATION:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_REQUEST_DEBUG_LINES:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_SEND_DESIRED_STATE:
		{
			if (m_selectedBody >= 0)
			{
				// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
				b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
				//  b3Printf("prepare control command for body %d", m_selectedBody);

				prepareControlCommand(command);

				b3SubmitClientCommand(m_physicsClientHandle, command);
			}
			break;
		}
		case CMD_RESET_SIMULATION:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_SEND_BULLET_DATA_STREAM:
		{
#if 0

            //this worked, but needs C-API and a streaming options, similar to debug lines
            command.m_type = buttonId;
            cl->enqueueCommand(command);
#endif

			break;
		}
		case CMD_CUSTOM_SET_GRAVITY:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
			b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_CUSTOM_SET_REALTIME_SIMULATION:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle);
			b3PhysicsParamSetRealTimeSimulation(commandHandle, 1);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		case CMD_CALCULATE_INVERSE_DYNAMICS:
		{
			if (m_selectedBody >= 0)
			{
				btAlignedObjectArray<double> jointPositionsQ;
				btAlignedObjectArray<double> jointVelocitiesQdot;
				btAlignedObjectArray<double> jointAccelerations;
				int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody);
				if (numJoints)
				{
					b3Printf("Compute inverse dynamics for joint accelerations:");
					jointPositionsQ.resize(numJoints);
					jointVelocitiesQdot.resize(numJoints);
					jointAccelerations.resize(numJoints);
					for (int i = 0; i < numJoints; i++)
					{
						jointAccelerations[i] = 100;
						b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]);
					}
					b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle,
																									  m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]);
					b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
				}
			}
			break;
		}
		case CMD_REQUEST_CONTACT_POINT_INFORMATION:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle);
			b3SetContactFilterBodyA(commandHandle, 0);
			b3SetContactFilterBodyB(commandHandle, 1);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_SAVE_WORLD:
		{
			b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_REQUEST_VISUAL_SHAPE_INFO:
		{
			if (m_selectedBody >= 0)
			{
				//request visual shape information
				b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(m_physicsClientHandle, m_selectedBody);
				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			}
			break;
		}
		case CMD_SET_SHADOW:
		{
			b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle);
			float viewMatrix[16];
			float projectionMatrix[16];
			m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix);
			m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix);

			b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix);
			b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight);
			bool hasShadow = true;
			b3RequestCameraImageSetShadow(commandHandle, hasShadow);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}
		case CMD_UPDATE_VISUAL_SHAPE:
		{
			int objectUniqueId = 0;
			int linkIndex = -1;
			int shapeIndex = -1;
			int textureIndex = -2;
			double rgbaColor[4] = {0.0, 1.0, 0.0, 1.0};
			b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape2(m_physicsClientHandle, objectUniqueId, linkIndex, shapeIndex);
			b3UpdateVisualShapeRGBAColor(commandHandle, rgbaColor);
			b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
			break;
		}

		default:
		{
			b3Error("Unknown buttonId");
			btAssert(0);
		}
	};
}
コード例 #9
0
ファイル: test.c プロジェクト: AndresTraks/bullet3
int main(int argc, char* argv[])
{
	int i, dofCount , posVarCount, dofIndex, ret ,numJoints, allowSharedMemoryInitialization=0;
	int timeout = MAX_TIMEOUT;
    int sensorJointIndexLeft=-1;
    int sensorJointIndexRight=-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;

	SharedMemoryCommand_t command;
	SharedMemoryStatus_t status;
	b3PhysicsClientHandle sm;
	
	b3Printf("timeout = %d\n",timeout);
    

	printf("hello world\n");

	sm = b3ConnectSharedMemory( allowSharedMemoryInitialization);
	if (b3CanSubmitCommand(sm))
	{
		ret = b3InitPhysicsParamCommand(&command);
		ret = b3PhysicsParamSetGravity(&command,  gravx,gravy, gravz);
		ret = b3PhysicsParamSetTimeStep(&command,  timeStep);
		ret = b3SubmitClientCommand(sm, &command);
		timeout = MAX_TIMEOUT;
		while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0)	{}
        b3Printf("timeout = %d\n",timeout);
        

		ret = b3LoadUrdfCommandInit(&command, urdfFileName);
		//setting the initial position, orientation and other arguments are optional
		startPosX =2;
		startPosY =3;
		startPosZ = 1;
		ret = b3LoadUrdfCommandSetStartPosition(&command, startPosX,startPosY,startPosZ);
		
		ret = b3SubmitClientCommand(sm, &command);
		timeout = MAX_TIMEOUT;
		while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0)	{}
		b3Printf("timeout = %d\n",timeout);
        
        
		numJoints = b3GetNumJoints(sm);
        for (i=0;i<numJoints;i++)
        {
            struct b3JointInfo jointInfo;
            b3GetJointInfo(sm,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))
        {
            ret = b3CreateSensorCommandInit(&command);

            if (imuLinkIndex>=0)
            {
				 ret = b3CreateSensorEnableIMUForLink(&command, imuLinkIndex, 1);
            }
            
            if (sensorJointIndexLeft>=0)
            {
			  ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1);
            }
            if(sensorJointIndexRight>=0)
            {
                ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexRight, 1);
            }
            ret = b3SubmitClientCommand(sm, &command);
            timeout = MAX_TIMEOUT;
            while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0)	{}
        }
        
        ret = b3CreateBoxShapeCommandInit(&command);
        ret = b3CreateBoxCommandSetStartPosition(&command, 0,0,-1);
        ret = b3CreateBoxCommandSetStartOrientation(&command,0,0,0,1);
        ret = b3CreateBoxCommandSetHalfExtents(&command, 10,10,1);
        ret = b3SubmitClientCommand(sm, &command);
        timeout = MAX_TIMEOUT;
        while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0)	{}
        
        
        b3RequestActualStateCommandInit(&command);
        ret = b3SubmitClientCommand(sm, &command);
        timeout = MAX_TIMEOUT;
		while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0)	{}

        posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
        dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU;
        
        b3Printf("posVarCount = %d\n",posVarCount);
        printf("dofCount = %d\n",dofCount);
        
        b3JointControlCommandInit(&command, CONTROL_MODE_VELOCITY);
        for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
        {
            b3JointControlSetDesiredVelocity(&command,dofIndex,1);
            b3JointControlSetMaximumForce(&command,dofIndex,100);
        }
		ret = b3SubmitClientCommand(sm, &command);
		timeout = MAX_TIMEOUT;
		while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0)	{}
        
        ///perform some simulation steps for testing
        for ( i=0;i<100;i++)
        {
            ret = b3InitStepSimulationCommand(&command);
            ret = b3SubmitClientCommand(sm, &command);
            timeout = MAX_TIMEOUT;
            while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0)	{}
        }
        
        b3RequestActualStateCommandInit(&command);
        ret = b3SubmitClientCommand(sm, &command);
        timeout = MAX_TIMEOUT;
		while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0)	{}
        
        if (sensorJointIndexLeft>=0)
        {
            b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
                status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+0],
                status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+1],
                status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+2]);
        }
        
        if (sensorJointIndexRight>=0)
        {
            b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
                     status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+0],
                     status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1],
                     status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]);
        }

	 ret = b3InitResetSimulationCommand(&command);
            ret = b3SubmitClientCommand(sm, &command);
            timeout = MAX_TIMEOUT;
            while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0)    {}
        
        
	}


	b3DisconnectSharedMemory(sm);
}