//#include "SharedMemoryCommands.h" #ifdef PHYSICS_SHARED_MEMORY #include "SharedMemory/PhysicsClientC_API.h" #endif //PHYSICS_SHARED_MEMORY #ifdef PHYSICS_LOOP_BACK #include "SharedMemory/PhysicsLoopBackC_API.h" #endif //PHYSICS_LOOP_BACK #ifdef PHYSICS_SERVER_DIRECT #include "SharedMemory/PhysicsDirectC_API.h" #endif //PHYSICS_SERVER_DIRECT #ifdef PHYSICS_IN_PROCESS_EXAMPLE_BROWSER #include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h" #endif//PHYSICS_IN_PROCESS_EXAMPLE_BROWSER #include "SharedMemory/SharedMemoryPublic.h" #include "Bullet3Common/b3Logging.h" #include #include #ifndef ENABLE_GTEST #include #define ASSERT_EQ(a,b) assert((a)==(b)); #else #define printf #endif void testSharedMemory(b3PhysicsClientHandle sm) { int i, dofCount , posVarCount, ret ,numJoints ; 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; 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; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); //setting the initial position, orientation and other arguments are optional startPosX =0; 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=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); 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=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); } #ifdef ENABLE_GTEST TEST(BulletPhysicsClientServerTest, DirectConnection) { b3PhysicsClientHandle sm = b3ConnectPhysicsDirect(); testSharedMemory(sm); } TEST(BulletPhysicsClientServerTest, LoopBackSharedMemory) { b3PhysicsClientHandle sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); testSharedMemory(sm); } #else 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 testSharedMemory(sm); } #endif