Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

small progress towards shared memory C-api and test.c.

Browse Source
This commit is contained in:
erwincoumans
2015-07-30 23:22:44 -07:00
parent 26531f3fbc
commit 19c5be5646
11 changed files with 346 additions and 256 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
build3/premake4.lua
View File

@@ -165,6 +165,7 @@ if not _OPTIONS["ios"] then
include "../examples/HelloWorld" include "../examples/HelloWorld"
include "../examples/BasicDemo" include "../examples/BasicDemo"
include "../test/SharedMemory"
if _OPTIONS["enet"] then if _OPTIONS["enet"] then
include "../examples/ThirdPartyLibs/enet" include "../examples/ThirdPartyLibs/enet"

193
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -126,9 +126,20 @@ bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus) bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus)
{ {
btAssert(m_data->m_testBlock1);
bool hasStatus = false; 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) 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); 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 //consume the command
switch (serverCmd.m_type) switch (serverCmd.m_type)
{ {
case CMD_CLIENT_COMMAND_COMPLETED:
{
b3Printf("Server completed command");
break;
}
case CMD_URDF_LOADING_COMPLETED: case CMD_URDF_LOADING_COMPLETED:
{ {
m_data->m_serverLoadUrdfOK = true; m_data->m_serverLoadUrdfOK = true;
@@ -240,10 +255,12 @@ bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverSt
} }
case CMD_DESIRED_STATE_RECEIVED_COMPLETED: case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{ {
b3Printf("Server received desired state");
break; break;
} }
case CMD_STEP_FORWARD_SIMULATION_COMPLETED: case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{ {
b3Printf("Server completed step simulation");
break; break;
} }
case CMD_URDF_LOADING_FAILED: case CMD_URDF_LOADING_FAILED:
@@ -349,171 +366,15 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const
bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command) 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) if (!m_data->m_waitingForServer)
{ {
//process command m_data->m_testBlock1->m_clientCommands[0] = command;
{ m_data->m_testBlock1->m_numClientCommands++;
m_data->m_waitingForServer = true; 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 (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
fread(m_data->m_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");
}
}
}
}
return true; return true;
}
return false;
} }

103
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1,6 +1,49 @@
#include "PhysicsClientC_API.h" #include "PhysicsClientC_API.h"
#include "PhysicsClient.h" #include "PhysicsClient.h"
#include "Bullet3Common/b3Scalar.h" #include "Bullet3Common/b3Scalar.h"
#include <string.h>
int b3LoadUrdfCommandInit(struct SharedMemoryCommand* command, const char* urdfFileName)
{
b3Assert(command);
command->m_type = CMD_LOAD_URDF;
int len = strlen(urdfFileName);
if (len<MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_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) int b3InitPhysicsParamCommand(struct SharedMemoryCommand* command)
{ {
@@ -20,6 +63,66 @@ int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gra
return 0; 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 #if 0
#include "SharedMemoryBlock.h" #include "SharedMemoryBlock.h"

16
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -6,12 +6,13 @@
#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name #define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
B3_DECLARE_HANDLE(b3PhysicsClientHandle); B3_DECLARE_HANDLE(b3PhysicsClientHandle);
B3_DECLARE_HANDLE(b3PhysicsRobotHandle);
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
b3PhysicsClientHandle b3ConnectSharedMemory(int memKey, int allowSharedMemoryInitialization); b3PhysicsClientHandle b3ConnectSharedMemory(int allowSharedMemoryInitialization);
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
@@ -23,10 +24,21 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, struct SharedMemoryC
int b3GetNumPoweredJoints(b3PhysicsClientHandle physClient); 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 b3InitPhysicsParamCommand(struct SharedMemoryCommand* command);
int b3PhysicsParamSetGravity(struct SharedMemoryCommand* command, double gravx,double gravy, double gravz); 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 #ifdef __cplusplus
} }

164
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -55,6 +55,20 @@ struct PhysicsServerInternalData
m_guiHelper(0) 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() void PhysicsServerSharedMemory::processClientCommands()
{ {
if (m_data->m_isConnected && m_data->m_testBlock1) 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]; const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
m_data->m_testBlock1->m_numProcessedClientCommands++; m_data->m_testBlock1->m_numProcessedClientCommands++;
//no timestamp yet
int timeStamp = 0;
//consume the command //consume the command
switch (clientCmd.m_type) switch (clientCmd.m_type)
{ {
@@ -317,35 +338,54 @@ void PhysicsServerSharedMemory::processClientCommands()
m_data->m_worldImporters.push_back(worldImporter); m_data->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); 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) 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); m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED; m_data->submitServerStatus(status);
} else } 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; break;
} }
case CMD_LOAD_URDF: 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; const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;
b3Printf("Processed CMD_LOAD_URDF:%s", urdfArgs.m_urdfFileName); 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 //load the actual URDF and send a report: completed or failed
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
btVector3(urdfArgs.m_initialPosition[0], initialPos,initialOrn,
urdfArgs.m_initialPosition[1], useMultiBody, useFixedBase);
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);
SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
if (completedOk) 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_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 } 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: case CMD_SEND_DESIRED_STATE:
{ {
//for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
//{
// m_testBlock1->m_desiredStateForceTorque[i] = 100;
//}
b3Printf("Processed CMD_SEND_DESIRED_STATE"); b3Printf("Processed CMD_SEND_DESIRED_STATE");
if (m_data->m_dynamicsWorld->getNumMultibodies()>0) if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{ {
@@ -449,11 +487,8 @@ void PhysicsServerSharedMemory::processClientCommands()
} }
} }
SharedMemoryStatus& status = m_data->createServerStatus(CMD_DESIRED_STATE_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
serverCmd.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
m_data->m_testBlock1->m_numServerCommands++;
break; break;
} }
case CMD_REQUEST_ACTUAL_STATE: case CMD_REQUEST_ACTUAL_STATE:
@@ -462,8 +497,7 @@ void PhysicsServerSharedMemory::processClientCommands()
if (m_data->m_dynamicsWorld->getNumMultibodies()>0) if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{ {
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0); btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; SharedMemoryStatus& serverCmd = m_data->createServerStatus(CMD_ACTUAL_STATE_UPDATE_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomQ = 0;
@@ -516,36 +550,16 @@ void PhysicsServerSharedMemory::processClientCommands()
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;
m_data->submitServerStatus(serverCmd);
} else } else
{ {
b3Warning("Request state but no multibody available"); 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;i<m_jointFeedbacks.size();i++)
{
printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
m_jointFeedbacks[i]->m_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; break;
} }
case CMD_STEP_FORWARD_SIMULATION: case CMD_STEP_FORWARD_SIMULATION:
@@ -554,10 +568,8 @@ void PhysicsServerSharedMemory::processClientCommands()
b3Printf("Step simulation request"); b3Printf("Step simulation request");
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime); m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime);
SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_STEP_FORWARD_SIMULATION_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_data->m_testBlock1->m_numServerCommands++;
break; break;
} }
@@ -575,11 +587,8 @@ void PhysicsServerSharedMemory::processClientCommands()
} }
SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; m_data->submitServerStatus(serverCmd);
m_data->m_testBlock1->m_numServerCommands++;
break; break;
}; };
@@ -589,9 +598,9 @@ void PhysicsServerSharedMemory::processClientCommands()
///@todo: implement this ///@todo: implement this
m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0)); m_data->m_dynamicsWorld->setGravity(btVector3(0,0,0));
SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0]; SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; m_data->submitServerStatus(serverCmd);
m_data->m_testBlock1->m_numServerCommands++;
break; break;
} }
@@ -601,6 +610,9 @@ void PhysicsServerSharedMemory::processClientCommands()
{ {
btAssert(0); btAssert(0);
//wantsShutdown = true; //wantsShutdown = true;
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
break; break;
} }
case CMD_CREATE_BOX_COLLISION_SHAPE: case CMD_CREATE_BOX_COLLISION_SHAPE:
@@ -618,22 +630,22 @@ void PhysicsServerSharedMemory::processClientCommands()
bool isDynamic = (mass>0); bool isDynamic = (mass>0);
worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); 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; SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->m_testBlock1->m_numServerCommands++; m_data->submitServerStatus(serverCmd);
break; break;
} }
default: default:
{ {
b3Error("Unsupported command encountered"); b3Error("Unknown command encountered");
btAssert(0);
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_UNKNOWN_COMMAND_FLUSHED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd);
} }
}; };
//process the command right now
} }
} }
} }

1
examples/SharedMemory/PhysicsServer.h
View File

@@ -9,6 +9,7 @@ protected:
void createJointMotors(class btMultiBody* body); void createJointMotors(class btMultiBody* body);
void releaseSharedMemory(); void releaseSharedMemory();
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn, bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,

2
examples/SharedMemory/SharedMemoryBlock.h
View File

@@ -30,7 +30,7 @@ struct SharedMemoryBlock
}; };
inline void InitSharedMemoryBlock(struct SharedMemoryBlock* sharedMemoryBlock) static void InitSharedMemoryBlock(struct SharedMemoryBlock* sharedMemoryBlock)
{ {
sharedMemoryBlock->m_numClientCommands = 0; sharedMemoryBlock->m_numClientCommands = 0;
sharedMemoryBlock->m_numServerCommands = 0; sharedMemoryBlock->m_numServerCommands = 0;

41
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -35,13 +35,21 @@ enum EnumSharedMemoryClientCommand
CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
CMD_SEND_DESIRED_STATE, CMD_SEND_DESIRED_STATE,
CMD_REQUEST_ACTUAL_STATE, CMD_REQUEST_ACTUAL_STATE,
CMD_STEP_FORWARD_SIMULATION, //includes CMD_REQUEST_STATE CMD_STEP_FORWARD_SIMULATION,
CMD_SHUTDOWN, CMD_SHUTDOWN,
CMD_MAX_CLIENT_COMMANDS CMD_MAX_CLIENT_COMMANDS
}; };
enum EnumSharedMemoryServerStatus 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_COMPLETED,
CMD_URDF_LOADING_FAILED, CMD_URDF_LOADING_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED, CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
@@ -50,6 +58,7 @@ enum EnumSharedMemoryServerStatus
CMD_RIGID_BODY_CREATION_COMPLETED, CMD_RIGID_BODY_CREATION_COMPLETED,
CMD_SET_JOINT_FEEDBACK_COMPLETED, CMD_SET_JOINT_FEEDBACK_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_COMPLETED, CMD_ACTUAL_STATE_UPDATE_COMPLETED,
CMD_ACTUAL_STATE_UPDATE_FAILED,
CMD_DESIRED_STATE_RECEIVED_COMPLETED, CMD_DESIRED_STATE_RECEIVED_COMPLETED,
CMD_STEP_FORWARD_SIMULATION_COMPLETED, CMD_STEP_FORWARD_SIMULATION_COMPLETED,
CMD_MAX_SERVER_COMMANDS CMD_MAX_SERVER_COMMANDS
@@ -60,6 +69,15 @@ enum EnumSharedMemoryServerStatus
#define MAX_NUM_SENSORS 1024 #define MAX_NUM_SENSORS 1024
#define MAX_URDF_FILENAME_LENGTH 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 struct UrdfArgs
{ {
@@ -120,7 +138,7 @@ struct SendDesiredStateArgs
}; };
enum EnumUpdateFlags enum EnumSimParamUpdateFlags
{ {
SIM_PARAM_UPDATE_DELTA_TIME=1, SIM_PARAM_UPDATE_DELTA_TIME=1,
SIM_PARAM_UPDATE_GRAVITY=2, SIM_PARAM_UPDATE_GRAVITY=2,
@@ -168,18 +186,19 @@ struct SharedMemoryCommand
int m_type; int m_type;
smUint64_t m_timeStamp; smUint64_t m_timeStamp;
int m_sequenceNumber; 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; //m_updateFlags is a bit fields to tell which parameters need updating
int m_updateFlags; //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;
int m_updateFlags;
union union
{ {
struct UrdfArgs m_urdfArguments; struct UrdfArgs m_urdfArguments;
struct InitPoseArgs m_initPoseArgs; struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments; struct BulletDataStreamArgs m_dataStreamArguments;
struct SendDesiredStateArgs m_sendDesiredStateCommandArgument; struct SendDesiredStateArgs m_sendDesiredStateCommandArgument;
struct RequestActualStateArgs m_requestActualStateInformationCommandArgument; struct RequestActualStateArgs m_requestActualStateInformationCommandArgument;
}; };
}; };
@@ -198,6 +217,8 @@ struct SharedMemoryStatus
}; };
}; };
typedef struct SharedMemoryStatus SharedMemoryStatus_t;
struct PoweredJointInfo struct PoweredJointInfo
{ {
char* m_linkName; char* m_linkName;

5
examples/SharedMemory/Win32SharedMemory.cpp
View File

@@ -77,15 +77,20 @@ void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre
} }
void Win32SharedMemory::releaseSharedMemory(int key, int size) void Win32SharedMemory::releaseSharedMemory(int key, int size)
{ {
if (m_internalData->m_buf) if (m_internalData->m_buf)
{ {
UnmapViewOfFile(m_internalData->m_buf); UnmapViewOfFile(m_internalData->m_buf);
m_internalData->m_buf=0; m_internalData->m_buf=0;
} }
if (m_internalData->m_hMapFile) if (m_internalData->m_hMapFile)
{ {
CloseHandle(m_internalData->m_hMapFile); CloseHandle(m_internalData->m_hMapFile);
m_internalData->m_hMapFile = 0;
} }
} }
Win32SharedMemoryServer::Win32SharedMemoryServer() Win32SharedMemoryServer::Win32SharedMemoryServer()

26
test/SharedMemory/premake4.lua Normal file
View File

@@ -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",
}

50
test/SharedMemory/test.c
View File

@@ -1,11 +1,59 @@
#include "SharedMemoryBlock.h" #include "SharedMemoryBlock.h"
#include "PhysicsClientC_API.h" #include "PhysicsClientC_API.h"
#include "SharedMemoryCommands.h"
struct test struct test
{ {
int unused;
}; };
#include <stdio.h> #include <stdio.h>
#define MAX_TIMEOUT 1024
int main(int argc, char* argv[]) 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);
} }