diff --git a/build3/premake4.lua b/build3/premake4.lua index d04b1274d..cb87cce0b 100644 --- a/build3/premake4.lua +++ b/build3/premake4.lua @@ -165,6 +165,7 @@ if not _OPTIONS["ios"] then include "../examples/HelloWorld" include "../examples/BasicDemo" + include "../test/SharedMemory" if _OPTIONS["enet"] then include "../examples/ThirdPartyLibs/enet" diff --git a/examples/SharedMemory/PhysicsClient.cpp b/examples/SharedMemory/PhysicsClient.cpp index bd86dce6c..b0e2a2c83 100644 --- a/examples/SharedMemory/PhysicsClient.cpp +++ b/examples/SharedMemory/PhysicsClient.cpp @@ -126,9 +126,20 @@ bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization) bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus) { - btAssert(m_data->m_testBlock1); bool hasStatus = false; + if (!m_data->m_testBlock1) + { + serverStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED; + return true; + } + + if (!m_data->m_waitingForServer) + { + serverStatus.m_type = CMD_WAITING_FOR_CLIENT_COMMAND; + return true; + } + if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands) { btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1); @@ -140,7 +151,11 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt //consume the command switch (serverCmd.m_type) { - + case CMD_CLIENT_COMMAND_COMPLETED: + { + b3Printf("Server completed command"); + break; + } case CMD_URDF_LOADING_COMPLETED: { m_data->m_serverLoadUrdfOK = true; @@ -240,10 +255,12 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt } case CMD_DESIRED_STATE_RECEIVED_COMPLETED: { + b3Printf("Server received desired state"); break; } case CMD_STEP_FORWARD_SIMULATION_COMPLETED: { + b3Printf("Server completed step simulation"); break; } case CMD_URDF_LOADING_FAILED: @@ -349,171 +366,15 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) { + ///at the moment we allow a maximum of 1 outstanding command, so we check for this + //once the server processed the command and returns a status, we clear the flag "m_data->m_waitingForServer" and allow submitting the next command if (!m_data->m_waitingForServer) - { - //process command - { - m_data->m_waitingForServer = true; - - switch (command.m_type) - { - - case CMD_LOAD_URDF: - { - if (!m_data->m_serverLoadUrdfOK) - { - m_data->m_testBlock1->m_clientCommands[0] = command; - - m_data->m_testBlock1->m_numClientCommands++; - b3Printf("Client created CMD_LOAD_URDF\n"); - } else - { - b3Warning("Server already loaded URDF, no client command submitted\n"); - } - break; - } - case CMD_CREATE_BOX_COLLISION_SHAPE: - { - if (m_data->m_serverLoadUrdfOK) - { - b3Printf("Requesting create box collision shape\n"); - m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_CREATE_BOX_COLLISION_SHAPE; - m_data->m_testBlock1->m_numClientCommands++; - } else - { - b3Warning("No URDF loaded\n"); - } - break; - } - case CMD_SEND_BULLET_DATA_STREAM: - { - b3Printf("Sending a Bullet Data Stream\n"); - ///The idea is to pass a stream of chunks from client to server - ///over shared memory. The server will process it - ///Initially we will just copy an entire .bullet file into shared - ///memory but we can also send individual chunks one at a time - ///so it becomes a streaming solution - ///In addition, we can make a separate API to create those chunks - ///if needed, instead of using a 3d modeler or the Bullet SDK btSerializer - - char relativeFileName[1024]; - const char* fileName = "slope.bullet"; - bool fileFound = b3ResourcePath::findResourcePath(fileName,relativeFileName,1024); - if (fileFound) - { - FILE *fp = fopen(relativeFileName, "rb"); - if (fp) - { - fseek(fp, 0L, SEEK_END); - int mFileLen = ftell(fp); - fseek(fp, 0L, SEEK_SET); - if (mFileLenm_testBlock1->m_bulletStreamDataClientToServer, mFileLen, 1, fp); - - fclose(fp); - - m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_BULLET_DATA_STREAM; - m_data->m_testBlock1->m_clientCommands[0].m_dataStreamArguments.m_streamChunkLength = mFileLen; - m_data->m_testBlock1->m_numClientCommands++; - b3Printf("Send bullet data stream command\n"); - } else - { - b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); - } - } else - { - b3Warning("Cannot open file %s\n", relativeFileName); - } - } else - { - b3Warning("Cannot find file %s\n", fileName); - } - - break; - } - - case CMD_INIT_POSE: - { - if (m_data->m_serverLoadUrdfOK) - { - b3Printf("Initialize Pose"); - m_data->m_testBlock1->m_clientCommands[0] = command; - m_data->m_testBlock1->m_numClientCommands++; - - } - break; - } - case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: - { - b3Printf("Send Physics Simulation Parameters"); - m_data->m_testBlock1->m_clientCommands[0] = command; - m_data->m_testBlock1->m_numClientCommands++; - break; - } - - case CMD_REQUEST_ACTUAL_STATE: - { - if (m_data->m_serverLoadUrdfOK) - { - b3Printf("Requesting actual state\n"); - m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE; - m_data->m_testBlock1->m_numClientCommands++; - - } else - { - b3Warning("No URDF loaded\n"); - } - break; - } - case CMD_SEND_DESIRED_STATE: - { - if (m_data->m_serverLoadUrdfOK) - { - b3Printf("Sending desired state (pos, vel, torque)\n"); - m_data->m_testBlock1->m_clientCommands[0] = command; - - m_data->m_testBlock1->m_numClientCommands++; - - } else - { - b3Warning("Cannot send CMD_SEND_DESIRED_STATE, no URDF loaded\n"); - } - break; - } - case CMD_STEP_FORWARD_SIMULATION: - { - if (m_data->m_serverLoadUrdfOK) - { - - m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION; - // m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; - m_data->m_testBlock1->m_numClientCommands++; - b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_data->m_counter++); - } else - { - b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n"); - } - break; - } - case CMD_SHUTDOWN: - { - - m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN; - m_data->m_testBlock1->m_numClientCommands++; - m_data->m_serverLoadUrdfOK = false; - b3Printf("client created CMD_SHUTDOWN\n"); - break; - } - default: - { - b3Error("unknown command requested\n"); - } - } - } - } - + { + m_data->m_testBlock1->m_clientCommands[0] = command; + m_data->m_testBlock1->m_numClientCommands++; + m_data->m_waitingForServer = true; return true; + } + return false; } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 4b5c20a9a..be1ec94b3 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1,6 +1,49 @@ #include "PhysicsClientC_API.h" #include "PhysicsClient.h" #include "Bullet3Common/b3Scalar.h" +#include + +int b3LoadUrdfCommandInit(struct SharedMemoryCommand* command, const char* urdfFileName) +{ + b3Assert(command); + command->m_type = CMD_LOAD_URDF; + int len = strlen(urdfFileName); + if (lenm_urdfArguments.m_urdfFileName,urdfFileName); + } else + { + command->m_urdfArguments.m_urdfFileName[0] = 0; + } + command->m_updateFlags = URDF_ARGS_FILE_NAME; + + return 0; +} + + + + +int b3LoadUrdfCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ) +{ + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_URDF); + command->m_urdfArguments.m_initialPosition[0] = startPosX; + command->m_urdfArguments.m_initialPosition[1] = startPosY; + command->m_urdfArguments.m_initialPosition[2] = startPosZ; + command->m_updateFlags|=URDF_ARGS_INITIAL_POSITION; + return 0; +} +int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW) +{ + b3Assert(command); + b3Assert(command->m_type == CMD_LOAD_URDF); + command->m_urdfArguments.m_initialOrientation[0] = startOrnX; + command->m_urdfArguments.m_initialOrientation[1] = startOrnY; + command->m_urdfArguments.m_initialOrientation[2] = startOrnZ; + command->m_urdfArguments.m_initialOrientation[3] = startOrnW; + command->m_updateFlags|=URDF_ARGS_INITIAL_ORIENTATION; + return 0; +} int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command) { @@ -20,6 +63,66 @@ int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gra return 0; } +int b3PhysicsParamSetTimeStep(struct SharedMemoryCommand* command, double timeStep) +{ + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_deltaTime = timeStep; + return 0; +} + +int b3InitStepSimulationCommand(struct SharedMemoryCommand* command) +{ + b3Assert(command); + command->m_type = CMD_STEP_FORWARD_SIMULATION; + command->m_updateFlags = 0; + + return 0; + +} + +b3PhysicsClientHandle b3ConnectSharedMemory( int allowSharedMemoryInitialization) +{ + PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory(); + cl->connect(allowSharedMemoryInitialization); + return (b3PhysicsClientHandle ) cl; +} + +void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient) +{ + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + delete cl; +} + +int b3ProcessServerStatus(b3PhysicsClientHandle physClient, struct SharedMemoryStatus* status) +{ + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + return (int)cl->processServerStatus(*status); +} + +int b3CanSubmitCommand(b3PhysicsClientHandle physClient) +{ + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + return (int)cl->canSubmitCommand(); +} + +int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryCommand* command) +{ + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + return (int)cl->submitClientCommand(*command); +} + +int b3GetNumJoints(b3PhysicsClientHandle physClient) +{ + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + return cl->getNumPoweredJoints(); +} + +void b3GetPoweredJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct PoweredJointInfo* info) +{ + PhysicsClientSharedMemory* cl = (PhysicsClientSharedMemory* ) physClient; + cl->getPoweredJointInfo(linkIndex,*info); +} + #if 0 #include "SharedMemoryBlock.h" diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index de90abe04..472b19a25 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -6,12 +6,13 @@ #define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name B3_DECLARE_HANDLE(b3PhysicsClientHandle); +B3_DECLARE_HANDLE(b3PhysicsRobotHandle); #ifdef __cplusplus extern "C" { #endif -b3PhysicsClientHandle b3ConnectSharedMemory(int memKey, int allowSharedMemoryInitialization); +b3PhysicsClientHandle b3ConnectSharedMemory(int allowSharedMemoryInitialization); void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); @@ -23,10 +24,21 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryC int b3GetNumPoweredJoints(b3PhysicsClientHandle physClient); -void b3GetPoweredJointInfo(int linkIndex, struct PoweredJointInfo* info); +void b3GetPoweredJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct PoweredJointInfo* info); int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command); int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz); +int b3PhysicsParamSetTimeStep(struct SharedMemoryCommand* command, double timeStep); + +int b3InitStepSimulationCommand(struct SharedMemoryCommand* command); + +int b3LoadUrdfCommandInit(struct SharedMemoryCommand* command, const char* urdfFileName); +///all those commands are optional, except for the *Init +int b3LoadUrdfCommandSetStartPosition(struct SharedMemoryCommand* command, double startPosX,double startPosY,double startPosZ); +int b3LoadUrdfCommandSetStartOrientation(struct SharedMemoryCommand* command, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); +int b3LoadUrdfCommandSetUseMultiBody(struct SharedMemoryCommand* command, int useMultiBody); +int b3LoadUrdfCommandSetUseFixedBase(struct SharedMemoryCommand* command, int useFixedBase); + #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 06b68f834..a09fc5608 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -55,6 +55,20 @@ struct PhysicsServerInternalData m_guiHelper(0) { } + + SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp) + { + SharedMemoryStatus& serverCmd =m_testBlock1->m_serverCommands[0]; + serverCmd .m_type = statusType; + serverCmd.m_sequenceNumber = sequenceNumber; + serverCmd.m_timeStamp = timeStamp; + return serverCmd; + } + void submitServerStatus(SharedMemoryStatus& status) + { + m_testBlock1->m_numServerCommands++; + } + }; @@ -292,6 +306,11 @@ bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& } + + + + + void PhysicsServerSharedMemory::processClientCommands() { if (m_data->m_isConnected && m_data->m_testBlock1) @@ -305,7 +324,9 @@ void PhysicsServerSharedMemory::processClientCommands() const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; m_data->m_testBlock1->m_numProcessedClientCommands++; - + //no timestamp yet + int timeStamp = 0; + //consume the command switch (clientCmd.m_type) { @@ -317,35 +338,54 @@ void PhysicsServerSharedMemory::processClientCommands() m_data->m_worldImporters.push_back(worldImporter); bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); - SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; - if (completedOk) { + SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED; + m_data->submitServerStatus(status); } else { - serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_FAILED; - + SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(status); } - m_data->m_testBlock1->m_numServerCommands++; + break; } case CMD_LOAD_URDF: { + //at the moment, we only load 1 urdf / robot + if (m_data->m_urdfLinkNameMapper.size()) + { + SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(status); + break; + } const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); + btAssert((clientCmd.m_updateFlags&URDF_ARGS_FILE_NAME) !=0); + btAssert(urdfArgs.m_urdfFileName); + btVector3 initialPos(0,0,0); + btQuaternion initialOrn(0,0,0,1); + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION) + { + initialPos[0] = urdfArgs.m_initialPosition[0]; + initialPos[1] = urdfArgs.m_initialPosition[1]; + initialPos[2] = urdfArgs.m_initialPosition[2]; + } + if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION) + { + initialOrn[0] = urdfArgs.m_initialOrientation[0]; + initialOrn[1] = urdfArgs.m_initialOrientation[1]; + initialOrn[2] = urdfArgs.m_initialOrientation[2]; + initialOrn[3] = urdfArgs.m_initialOrientation[3]; + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? urdfArgs.m_useMultiBody : true; + bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? urdfArgs.m_useFixedBase: false; //load the actual URDF and send a report: completed or failed bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, - btVector3(urdfArgs.m_initialPosition[0], - urdfArgs.m_initialPosition[1], - urdfArgs.m_initialPosition[2]), - btQuaternion(urdfArgs.m_initialOrientation[0], - urdfArgs.m_initialOrientation[1], - urdfArgs.m_initialOrientation[2], - urdfArgs.m_initialOrientation[3]), - urdfArgs.m_useMultiBody, urdfArgs.m_useFixedBase); + initialPos,initialOrn, + useMultiBody, useFixedBase); SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; if (completedOk) @@ -354,13 +394,16 @@ void PhysicsServerSharedMemory::processClientCommands() { serverCmd.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } - serverCmd.m_type =CMD_URDF_LOADING_COMPLETED; + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(status); + } else { - serverCmd.m_type =CMD_URDF_LOADING_FAILED; - + SharedMemoryStatus& status = m_data->createServerStatus(CMD_URDF_LOADING_FAILED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(status); } - m_data->m_testBlock1->m_numServerCommands++; + @@ -368,11 +411,6 @@ void PhysicsServerSharedMemory::processClientCommands() } case CMD_SEND_DESIRED_STATE: { - //for (int i=0;im_desiredStateForceTorque[i] = 100; - //} - b3Printf("Processed CMD_SEND_DESIRED_STATE"); if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { @@ -449,11 +487,8 @@ void PhysicsServerSharedMemory::processClientCommands() } } - - - SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; - serverCmd.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; - m_data->m_testBlock1->m_numServerCommands++; + SharedMemoryStatus& status = m_data->createServerStatus(CMD_DESIRED_STATE_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(status); break; } case CMD_REQUEST_ACTUAL_STATE: @@ -462,8 +497,7 @@ void PhysicsServerSharedMemory::processClientCommands() if (m_data->m_dynamicsWorld->getNumMultibodies()>0) { btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); - SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; - serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0; int totalDegreeOfFreedomQ = 0; @@ -516,36 +550,16 @@ void PhysicsServerSharedMemory::processClientCommands() serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + m_data->submitServerStatus(serverCmd); } else { + b3Warning("Request state but no multibody available"); - //rigid bodies? + SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_FAILED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(serverCmd); } -/* - //now we send back the actual q, q' and force/torque and IMU sensor values - for (int i=0;im_appliedForceBodyA.x(), - m_jointFeedbacks[i]->m_appliedForceBodyA.y(), - m_jointFeedbacks[i]->m_appliedForceBodyA.z(), - m_jointFeedbacks[i]->m_appliedTorqueBodyA.x(), - m_jointFeedbacks[i]->m_appliedTorqueBodyA.y(), - m_jointFeedbacks[i]->m_appliedTorqueBodyA.z(), - m_jointFeedbacks[i]->m_appliedForceBodyB.x(), - m_jointFeedbacks[i]->m_appliedForceBodyB.y(), - m_jointFeedbacks[i]->m_appliedForceBodyB.z(), - m_jointFeedbacks[i]->m_appliedTorqueBodyB.x(), - m_jointFeedbacks[i]->m_appliedTorqueBodyB.y(), - m_jointFeedbacks[i]->m_appliedTorqueBodyB.z()); - } - */ - - m_data->m_testBlock1->m_numServerCommands++; - - break; } case CMD_STEP_FORWARD_SIMULATION: @@ -554,10 +568,8 @@ void PhysicsServerSharedMemory::processClientCommands() b3Printf("Step simulation request"); m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime); - SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; - - serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; - m_data->m_testBlock1->m_numServerCommands++; + SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_STEP_FORWARD_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(serverCmd); break; } @@ -575,11 +587,8 @@ void PhysicsServerSharedMemory::processClientCommands() } - SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; - serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; - m_data->m_testBlock1->m_numServerCommands++; - - + SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(serverCmd); break; }; @@ -589,9 +598,9 @@ void PhysicsServerSharedMemory::processClientCommands() ///@todo: implement this m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); - SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; - serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; - m_data->m_testBlock1->m_numServerCommands++; + SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(serverCmd); + break; } @@ -601,6 +610,9 @@ void PhysicsServerSharedMemory::processClientCommands() { btAssert(0); //wantsShutdown = true; + SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(serverCmd); + break; } case CMD_CREATE_BOX_COLLISION_SHAPE: @@ -618,22 +630,22 @@ void PhysicsServerSharedMemory::processClientCommands() bool isDynamic = (mass>0); worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; - serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; - m_data->m_testBlock1->m_numServerCommands++; + + SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(serverCmd); + break; } default: { - b3Error("Unsupported command encountered"); - btAssert(0); + b3Error("Unknown command encountered"); + + SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_UNKNOWN_COMMAND_FLUSHED,clientCmd.m_sequenceNumber,timeStamp); + m_data->submitServerStatus(serverCmd); + } }; - - - //process the command right now - } } } diff --git a/examples/SharedMemory/PhysicsServer.h b/examples/SharedMemory/PhysicsServer.h index de97330fd..2b326cbb6 100644 --- a/examples/SharedMemory/PhysicsServer.h +++ b/examples/SharedMemory/PhysicsServer.h @@ -9,6 +9,7 @@ protected: void createJointMotors(class btMultiBody* body); + void releaseSharedMemory(); bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, diff --git a/examples/SharedMemory/SharedMemoryBlock.h b/examples/SharedMemory/SharedMemoryBlock.h index b3d2709d0..f922a4f26 100644 --- a/examples/SharedMemory/SharedMemoryBlock.h +++ b/examples/SharedMemory/SharedMemoryBlock.h @@ -30,7 +30,7 @@ struct SharedMemoryBlock }; -inline void InitSharedMemoryBlock(struct SharedMemoryBlock* sharedMemoryBlock) +static void InitSharedMemoryBlock(struct SharedMemoryBlock* sharedMemoryBlock) { sharedMemoryBlock->m_numClientCommands = 0; sharedMemoryBlock->m_numServerCommands = 0; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5a58b119d..c399fcda0 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -35,13 +35,21 @@ enum EnumSharedMemoryClientCommand CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, CMD_SEND_DESIRED_STATE, CMD_REQUEST_ACTUAL_STATE, - CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE + CMD_STEP_FORWARD_SIMULATION, CMD_SHUTDOWN, CMD_MAX_CLIENT_COMMANDS }; enum EnumSharedMemoryServerStatus { + CMD_SHARED_MEMORY_NOT_INITIALIZED, + CMD_WAITING_FOR_CLIENT_COMMAND, + + //CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client + CMD_CLIENT_COMMAND_COMPLETED, + //the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED' + CMD_UNKNOWN_COMMAND_FLUSHED, + CMD_URDF_LOADING_COMPLETED, CMD_URDF_LOADING_FAILED, CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, @@ -50,6 +58,7 @@ enum EnumSharedMemoryServerStatus CMD_RIGID_BODY_CREATION_COMPLETED, CMD_SET_JOINT_FEEDBACK_COMPLETED, CMD_ACTUAL_STATE_UPDATE_COMPLETED, + CMD_ACTUAL_STATE_UPDATE_FAILED, CMD_DESIRED_STATE_RECEIVED_COMPLETED, CMD_STEP_FORWARD_SIMULATION_COMPLETED, CMD_MAX_SERVER_COMMANDS @@ -60,6 +69,15 @@ enum EnumSharedMemoryServerStatus #define MAX_NUM_SENSORS 1024 #define MAX_URDF_FILENAME_LENGTH 1024 +enum EnumUrdfArgsUpdateFlags +{ + URDF_ARGS_FILE_NAME=1, + URDF_ARGS_INITIAL_POSITION=2, + URDF_ARGS_INITIAL_ORIENTATION=4, + URDF_ARGS_USE_MULTIBODY=8, + URDF_ARGS_USE_FIXED_BASE=16, +}; + struct UrdfArgs { @@ -120,7 +138,7 @@ struct SendDesiredStateArgs }; -enum EnumUpdateFlags +enum EnumSimParamUpdateFlags { SIM_PARAM_UPDATE_DELTA_TIME=1, SIM_PARAM_UPDATE_GRAVITY=2, @@ -168,18 +186,19 @@ struct SharedMemoryCommand int m_type; smUint64_t m_timeStamp; int m_sequenceNumber; - //a bit fields to tell which parameters need updating - //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; - int m_updateFlags; + + //m_updateFlags is a bit fields to tell which parameters need updating + //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS; + int m_updateFlags; union { struct UrdfArgs m_urdfArguments; - struct InitPoseArgs m_initPoseArgs; - struct SendPhysicsSimulationParameters m_physSimParamArgs; - struct BulletDataStreamArgs m_dataStreamArguments; - struct SendDesiredStateArgs m_sendDesiredStateCommandArgument; - struct RequestActualStateArgs m_requestActualStateInformationCommandArgument; + struct InitPoseArgs m_initPoseArgs; + struct SendPhysicsSimulationParameters m_physSimParamArgs; + struct BulletDataStreamArgs m_dataStreamArguments; + struct SendDesiredStateArgs m_sendDesiredStateCommandArgument; + struct RequestActualStateArgs m_requestActualStateInformationCommandArgument; }; }; @@ -198,6 +217,8 @@ struct SharedMemoryStatus }; }; +typedef struct SharedMemoryStatus SharedMemoryStatus_t; + struct PoweredJointInfo { char* m_linkName; diff --git a/examples/SharedMemory/Win32SharedMemory.cpp b/examples/SharedMemory/Win32SharedMemory.cpp index a62e717e2..e030104fc 100644 --- a/examples/SharedMemory/Win32SharedMemory.cpp +++ b/examples/SharedMemory/Win32SharedMemory.cpp @@ -77,15 +77,20 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre } void Win32SharedMemory::releaseSharedMemory(int key, int size) { + if (m_internalData->m_buf) { + UnmapViewOfFile(m_internalData->m_buf); m_internalData->m_buf=0; } + if (m_internalData->m_hMapFile) { CloseHandle(m_internalData->m_hMapFile); + m_internalData->m_hMapFile = 0; } + } Win32SharedMemoryServer::Win32SharedMemoryServer() diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua new file mode 100644 index 000000000..de781f51e --- /dev/null +++ b/test/SharedMemory/premake4.lua @@ -0,0 +1,26 @@ + project ("Test_SharedMemoryPhysicsClient") + + language "C++" + kind "ConsoleApp" + + includedirs {"../../src", "../../examples/SharedMemory"} + links { + "BulletFileLoader", + "Bullet3Common", + "LinearMath" + } + + files { + "test.c", + "../../examples/SharedMemory/PhysicsClient.cpp", + "../../examples/SharedMemory/PhysicsClient.h", + "../../examples/SharedMemory/PhysicsClientC_API.cpp", + "../../examples/SharedMemory/PhysicsClientC_API.h", + "../../examples/SharedMemory/Win32SharedMemory.cpp", + "../../examples/SharedMemory/Win32SharedMemory.h", + "../../examples/SharedMemory/PosixSharedMemory.cpp", + "../../examples/SharedMemory/PosixSharedMemory.h", + "../../examples/Utils/b3ResourcePath.cpp", + "../../examples/Utils/b3ResourcePath.h", + } + \ No newline at end of file diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 894fe6ef0..65837e8b2 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -1,11 +1,59 @@ #include "SharedMemoryBlock.h" #include "PhysicsClientC_API.h" +#include "SharedMemoryCommands.h" struct test { + int unused; }; + #include +#define MAX_TIMEOUT 1024 + int main(int argc, char* argv[]) { -printf("hello world\n"); + int ret ,allowSharedMemoryInitialization=0; + int timeout = MAX_TIMEOUT; + const char* urdfFileName = "r2d2.urdf"; + double gravx=0, gravy=0, gravz=0; + double timeStep = 1./60.; + double startPosX, startPosY,startPosZ; + + SharedMemoryCommand_t command; + SharedMemoryStatus_t status; + + b3PhysicsClientHandle sm; + 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) {} + + + 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) {} + + + + + ret = b3InitStepSimulationCommand(&command); + ret = b3SubmitClientCommand(sm, &command); + timeout = MAX_TIMEOUT; + while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} + } + b3DisconnectSharedMemory(sm); }