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

Refactor of PhysicsClient/PhysicsServer, to separate from the example browser code.

Browse Source 
(as usual, work-in-progress)
This commit is contained in:
erwin coumans
2015-07-14 15:30:17 -07:00
parent 2e0da2b7a1
commit 54a76f6e0c
16 changed files with 747 additions and 470 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -16,9 +16,11 @@ SET(App_ExampleBrowser_SRCS
ExampleEntries.cpp ExampleEntries.cpp
ExampleEntries.h ExampleEntries.h
../SharedMemory/PhysicsServer.cpp ../SharedMemory/PhysicsServer.cpp
../SharedMemory/PhysicsClient.cpp ../SharedMemory/PhysicsClient.cpp
../SharedMemory/PosixSharedMemory.cpp ../SharedMemory/PhysicsServerExample.cpp
../SharedMemory/Win32SharedMemory.cpp ../SharedMemory/PhysicsClientExample.cpp
../SharedMemory/PosixSharedMemory.cpp
../SharedMemory/Win32SharedMemory.cpp
../BasicDemo/BasicExample.cpp ../BasicDemo/BasicExample.cpp
../BasicDemo/BasicExample.h ../BasicDemo/BasicExample.h
../ForkLift/ForkLiftDemo.cpp ../ForkLift/ForkLiftDemo.cpp

4
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -31,8 +31,8 @@
#include "../FractureDemo/FractureDemo.h" #include "../FractureDemo/FractureDemo.h"
#include "../DynamicControlDemo/MotorDemo.h" #include "../DynamicControlDemo/MotorDemo.h"
#include "../RollingFrictionDemo/RollingFrictionDemo.h" #include "../RollingFrictionDemo/RollingFrictionDemo.h"
#include "../SharedMemory/PhysicsServer.h" #include "../SharedMemory/PhysicsServerExample.h"
#include "../SharedMemory/PhysicsClient.h" #include "../SharedMemory/PhysicsClientExample.h"
#include "../Constraints/TestHingeTorque.h" #include "../Constraints/TestHingeTorque.h"

2
examples/ExampleBrowser/premake4.lua
View File

@@ -50,6 +50,8 @@
files { files {
"**.cpp", "**.cpp",
"**.h", "**.h",
"../SharedMemory/PhysicsServerExample.cpp",
"../SharedMemory/PhysicsClientExample.cpp",
"../SharedMemory/PhysicsServer.cpp", "../SharedMemory/PhysicsServer.cpp",
"../SharedMemory/PhysicsClient.cpp", "../SharedMemory/PhysicsClient.cpp",
"../SharedMemory/PosixSharedMemory.cpp", "../SharedMemory/PosixSharedMemory.cpp",

359
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -1,179 +1,113 @@
#include "PhysicsClient.h" #include "PhysicsClient.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "PosixSharedMemory.h" #include "PosixSharedMemory.h"
#include "Win32SharedMemory.h" #include "Win32SharedMemory.h"
#include "SharedMemoryCommon.h" #include "LinearMath/btAlignedObjectArray.h"
#include "../CommonInterfaces/CommonParameterInterface.h" #include "Bullet3Common/b3Logging.h"
#include "../Utils/b3ResourcePath.h" #include "../Utils/b3ResourcePath.h"
#include "../Extras/Serialize/BulletFileLoader/btBulletFile.h" #include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
class PhysicsClient : public SharedMemoryCommon
struct PhysicsClientSharedMemoryInternalData
{ {
protected:
SharedMemoryInterface* m_sharedMemory; SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1; SharedMemoryExampleData* m_testBlock1;
int m_counter;
bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests;
int m_counter;
bool m_serverLoadUrdfOK; bool m_serverLoadUrdfOK;
bool m_isConnected;
bool m_waitingForServer; bool m_waitingForServer;
void processServerCommands(); PhysicsClientSharedMemoryInternalData()
void createClientCommand(); :m_sharedMemory(0),
m_testBlock1(0),
m_counter(0),
void createButton(const char* name, int id, bool isTrigger ); m_serverLoadUrdfOK(false),
m_isConnected(false),
public: m_waitingForServer(false)
PhysicsClient(GUIHelperInterface* helper);
virtual ~PhysicsClient();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{ {
float dist = 1;
float pitch = 50;
float yaw = 35;
float targetPos[3]={-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
} }
virtual bool wantsTermination()
{
return m_wantsTermination;
}
void submitCommand(int command);
void processServerStatus();
bool canSubmitCommand() const;
}; };
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{ {
PhysicsClient* cl = (PhysicsClient*) userPtr; m_data = new PhysicsClientSharedMemoryInternalData;
switch (buttonId)
{
case CMD_LOAD_URDF:
case CMD_CREATE_BOX_COLLISION_SHAPE:
case CMD_REQUEST_ACTUAL_STATE:
case CMD_STEP_FORWARD_SIMULATION:
case CMD_SHUTDOWN:
case CMD_SEND_DESIRED_STATE:
case CMD_SEND_BULLET_DATA_STREAM:
{
cl->submitCommand(buttonId);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
void PhysicsClient::submitCommand(int command)
{
m_userCommandRequests.push_back(command);
b3Printf("User submitted command request %d (outstanding %d)\n",command, m_userCommandRequests.size());
}
PhysicsClient::PhysicsClient(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_testBlock1(0),
m_counter(0),
m_wantsTermination(false),
m_serverLoadUrdfOK(false),
m_waitingForServer(false)
{
b3Printf("Started PhysicsClient\n");
#ifdef _WIN32 #ifdef _WIN32
m_sharedMemory = new Win32SharedMemoryClient(); m_data->m_sharedMemory = new Win32SharedMemoryClient();
#else #else
m_sharedMemory = new PosixSharedMemory(); m_data->m_sharedMemory = new PosixSharedMemory();
#endif #endif
} }
PhysicsClient::~PhysicsClient() PhysicsClientSharedMemory::~PhysicsClientSharedMemory()
{ {
b3Printf("~PhysicsClient\n"); m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); delete m_data->m_sharedMemory;
delete m_sharedMemory; delete m_data;
} }
void PhysicsClient::createButton(const char* name, int buttonId, bool isTrigger ) bool PhysicsClientSharedMemory::isConnected() const
{ {
ButtonParams button(name,buttonId, isTrigger); return m_data->m_isConnected ;
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
} }
void PhysicsClient::initPhysics()
bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
{ {
if (m_guiHelper && m_guiHelper->getParameterInterface()) bool allowCreation = true;
{ m_data->m_testBlock1 = (SharedMemoryExampleData*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation);
bool isTrigger = false;
if (m_data->m_testBlock1)
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
} else
{
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
}
m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
if (m_testBlock1)
{ {
// btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER); if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{ {
b3Error("Error: please start server before client\n"); if (allowSharedMemoryInitialization)
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); {
m_testBlock1 = 0; InitSharedMemoryExampleData(m_data->m_testBlock1);
b3Printf("Created and initialized shared memory block");
m_data->m_isConnected = true;
} else
{
b3Error("Error: please start server before client\n");
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
return false;
}
} else } else
{ {
b3Printf("Shared Memory status is OK\n"); b3Printf("Connected to existing shared memory, status OK.\n");
m_data->m_isConnected = true;
} }
} else } else
{ {
m_wantsTermination = true; b3Error("Cannot connect to shared memory");
return false;
} }
return true;
} }
void PhysicsClient::processServerCommands()
{
btAssert(m_testBlock1);
if (m_testBlock1->m_numServerCommands> m_testBlock1->m_numProcessedServerCommands)
void PhysicsClientSharedMemory::processServerStatus()
{
btAssert(m_data->m_testBlock1);
if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands)
{ {
btAssert(m_testBlock1->m_numServerCommands==m_testBlock1->m_numProcessedServerCommands+1); btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1);
const SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; const SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
//consume the command //consume the command
switch (serverCmd.m_type) switch (serverCmd.m_type)
@@ -181,12 +115,12 @@ void PhysicsClient::processServerCommands()
case CMD_URDF_LOADING_COMPLETED: case CMD_URDF_LOADING_COMPLETED:
{ {
m_serverLoadUrdfOK = true; m_data->m_serverLoadUrdfOK = true;
b3Printf("Server loading the URDF OK\n"); b3Printf("Server loading the URDF OK\n");
if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0) if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0)
{ {
bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength); bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf->setFileDNAisMemoryDNA(); bf->setFileDNAisMemoryDNA();
bf->parse(false); bf->parse(false);
for (int i=0;i<bf->m_multiBodies.size();i++) for (int i=0;i<bf->m_multiBodies.size();i++)
@@ -195,7 +129,7 @@ void PhysicsClient::processServerCommands()
if ((flag&bParse::FD_DOUBLE_PRECISION)!=0) if ((flag&bParse::FD_DOUBLE_PRECISION)!=0)
{ {
btMultiBodyDoubleData* mb = (btMultiBodyDoubleData*)bf->m_multiBodies[i]; Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i];
if (mb->m_baseName) if (mb->m_baseName)
{ {
b3Printf("mb->m_baseName = %s\n",mb->m_baseName); b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
@@ -213,7 +147,7 @@ void PhysicsClient::processServerCommands()
} }
} else } else
{ {
btMultiBodyFloatData* mb = (btMultiBodyFloatData*) bf->m_multiBodies[i]; Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i];
if (mb->m_baseName) if (mb->m_baseName)
{ {
b3Printf("mb->m_baseName = %s\n",mb->m_baseName); b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
@@ -247,7 +181,7 @@ void PhysicsClient::processServerCommands()
case CMD_URDF_LOADING_FAILED: case CMD_URDF_LOADING_FAILED:
{ {
b3Printf("Server failed loading the URDF...\n"); b3Printf("Server failed loading the URDF...\n");
m_serverLoadUrdfOK = false; m_data->m_serverLoadUrdfOK = false;
break; break;
} }
@@ -272,8 +206,8 @@ void PhysicsClient::processServerCommands()
{ {
b3Printf("Received actual state\n"); b3Printf("Received actual state\n");
int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ; int numQ = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU; int numU = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("size Q = %d, size U = %d\n", numQ,numU); b3Printf("size Q = %d, size U = %d\n", numQ,numU);
char msg[1024]; char msg[1024];
@@ -283,10 +217,10 @@ void PhysicsClient::processServerCommands()
{ {
if (i<numQ-1) if (i<numQ-1)
{ {
sprintf(msg,"%s%f,",msg,m_testBlock1->m_actualStateQ[i]); sprintf(msg,"%s%f,",msg,m_data->m_testBlock1->m_actualStateQ[i]);
} else } else
{ {
sprintf(msg,"%s%f",msg,m_testBlock1->m_actualStateQ[i]); sprintf(msg,"%s%f",msg,m_data->m_testBlock1->m_actualStateQ[i]);
} }
} }
sprintf(msg,"%s]",msg); sprintf(msg,"%s]",msg);
@@ -303,61 +237,53 @@ void PhysicsClient::processServerCommands()
}; };
m_testBlock1->m_numProcessedServerCommands++; m_data->m_testBlock1->m_numProcessedServerCommands++;
//we don't have more than 1 command outstanding (in total, either server or client) //we don't have more than 1 command outstanding (in total, either server or client)
btAssert(m_testBlock1->m_numProcessedServerCommands == m_testBlock1->m_numServerCommands); btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == m_data->m_testBlock1->m_numServerCommands);
if (m_testBlock1->m_numServerCommands == m_testBlock1->m_numProcessedServerCommands) if (m_data->m_testBlock1->m_numServerCommands == m_data->m_testBlock1->m_numProcessedServerCommands)
{ {
m_waitingForServer = false; m_data->m_waitingForServer = false;
} else } else
{ {
m_waitingForServer = true; m_data->m_waitingForServer = true;
} }
} }
} }
void PhysicsClient::createClientCommand() bool PhysicsClientSharedMemory::canSubmitCommand() const
{ {
if (!m_waitingForServer) return (m_data->m_isConnected && !m_data->m_waitingForServer);
}
bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command)
{
if (!m_data->m_waitingForServer)
{ {
//process outstanding requests //process command
if (m_userCommandRequests.size())
{ {
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); m_data->m_waitingForServer = true;
int command = m_userCommandRequests[0];
//don't use 'remove' because it will re-order the commands
//m_userCommandRequests.remove(command);
//pop_front
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back(); switch (command.m_type)
m_waitingForServer = true;
switch (command)
{ {
case CMD_LOAD_URDF: case CMD_LOAD_URDF:
{ {
if (!m_serverLoadUrdfOK) if (!m_data->m_serverLoadUrdfOK)
{ {
m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF; m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf"); sprintf(m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[0] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[0] = 0.0;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[1] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[1] = 0.0;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[0] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[0] = 0.0;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[1] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[1] = 0.0;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true; m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
b3Printf("Client created CMD_LOAD_URDF\n"); b3Printf("Client created CMD_LOAD_URDF\n");
} else } else
{ {
@@ -367,11 +293,11 @@ void PhysicsClient::createClientCommand()
} }
case CMD_CREATE_BOX_COLLISION_SHAPE: case CMD_CREATE_BOX_COLLISION_SHAPE:
{ {
if (m_serverLoadUrdfOK) if (m_data->m_serverLoadUrdfOK)
{ {
b3Printf("Requesting create box collision shape\n"); b3Printf("Requesting create box collision shape\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_CREATE_BOX_COLLISION_SHAPE; m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
} else } else
{ {
b3Warning("No URDF loaded\n"); b3Warning("No URDF loaded\n");
@@ -403,13 +329,13 @@ void PhysicsClient::createClientCommand()
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{ {
fread(m_testBlock1->m_bulletStreamDataClientToServer, mFileLen, 1, fp); fread(m_data->m_testBlock1->m_bulletStreamDataClientToServer, mFileLen, 1, fp);
fclose(fp); fclose(fp);
m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_BULLET_DATA_STREAM; m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_BULLET_DATA_STREAM;
m_testBlock1->m_clientCommands[0].m_dataStreamArguments.m_streamChunkLength = mFileLen; m_data->m_testBlock1->m_clientCommands[0].m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
b3Printf("Send bullet data stream command\n"); b3Printf("Send bullet data stream command\n");
} else } else
{ {
@@ -428,11 +354,11 @@ void PhysicsClient::createClientCommand()
} }
case CMD_REQUEST_ACTUAL_STATE: case CMD_REQUEST_ACTUAL_STATE:
{ {
if (m_serverLoadUrdfOK) if (m_data->m_serverLoadUrdfOK)
{ {
b3Printf("Requesting actual state\n"); b3Printf("Requesting actual state\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE; m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
} else } else
{ {
@@ -442,10 +368,10 @@ void PhysicsClient::createClientCommand()
} }
case CMD_SEND_DESIRED_STATE: case CMD_SEND_DESIRED_STATE:
{ {
if (m_serverLoadUrdfOK) if (m_data->m_serverLoadUrdfOK)
{ {
b3Printf("Sending desired state (pos, vel, torque)\n"); b3Printf("Sending desired state (pos, vel, torque)\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_DESIRED_STATE; m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_DESIRED_STATE;
//todo: expose a drop box in the GUI for this //todo: expose a drop box in the GUI for this
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
@@ -453,20 +379,20 @@ void PhysicsClient::createClientCommand()
{ {
case CONTROL_MODE_VELOCITY: case CONTROL_MODE_VELOCITY:
{ {
m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY; m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++) for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{ {
m_testBlock1->m_desiredStateQdot[i] = 1; m_data->m_testBlock1->m_desiredStateQdot[i] = 1;
m_testBlock1->m_desiredStateForceTorque[i] = 100; m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
} }
break; break;
} }
case CONTROL_MODE_TORQUE: case CONTROL_MODE_TORQUE:
{ {
m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_TORQUE; m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_TORQUE;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++) for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{ {
m_testBlock1->m_desiredStateForceTorque[i] = 100; m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
} }
break; break;
} }
@@ -477,7 +403,7 @@ void PhysicsClient::createClientCommand()
} }
} }
m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
} else } else
{ {
@@ -487,13 +413,13 @@ void PhysicsClient::createClientCommand()
} }
case CMD_STEP_FORWARD_SIMULATION: case CMD_STEP_FORWARD_SIMULATION:
{ {
if (m_serverLoadUrdfOK) if (m_data->m_serverLoadUrdfOK)
{ {
m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION; m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.; m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++); b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_data->m_counter++);
} else } else
{ {
b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n"); b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n");
@@ -502,10 +428,10 @@ void PhysicsClient::createClientCommand()
} }
case CMD_SHUTDOWN: case CMD_SHUTDOWN:
{ {
m_wantsTermination = true;
m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN; m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
m_serverLoadUrdfOK = false; m_data->m_serverLoadUrdfOK = false;
b3Printf("client created CMD_SHUTDOWN\n"); b3Printf("client created CMD_SHUTDOWN\n");
break; break;
} }
@@ -516,28 +442,7 @@ void PhysicsClient::createClientCommand()
} }
} }
} }
} return true;
void PhysicsClient::stepSimulation(float deltaTime)
{
// btAssert(m_testBlock1);
if (m_testBlock1)
{
processServerCommands();
if (!m_waitingForServer)
{
createClientCommand();
}
}
} }
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options)
{
return new PhysicsClient(options.m_guiHelper);
}

37
examples/SharedMemory/PhysicsClient.h
View File

@@ -1,6 +1,35 @@
#ifndef PHYSICS_CLIENT_H #ifndef BT_PHYSICS_CLIENT_API_H
#define PHYSICS_CLIENT_H #define BT_PHYSICS_CLIENT_API_H
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options); #include "SharedMemoryCommands.h"
#endif
class PhysicsClientSharedMemory //: public CommonPhysicsClientInterface
{
struct PhysicsClientSharedMemoryInternalData* m_data;
protected:
public:
PhysicsClientSharedMemory();
virtual ~PhysicsClientSharedMemory();
//todo: implement 'allocateSharedMemory' from client side in 'connect' call
virtual bool connect(bool allowSharedMemoryInitialization = true);
virtual bool isConnected() const;
virtual void processServerStatus();
virtual bool getLastServerStatus(ServerStatus& status)
{
return false;
}
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const SharedMemoryCommand& command);
};
#endif //BT_PHYSICS_CLIENT_API_H

173
examples/SharedMemory/PhysicsClientExample.cpp Normal file
View File

@@ -0,0 +1,173 @@
#include "PhysicsClientExample.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "SharedMemoryCommon.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "PhysicsClient.h"
#include "SharedMemoryCommands.h"
class PhysicsClientExample : public SharedMemoryCommon
{
protected:
PhysicsClientSharedMemory m_physicsClient;
bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests;
void createButton(const char* name, int id, bool isTrigger );
public:
PhysicsClientExample(GUIHelperInterface* helper);
virtual ~PhysicsClientExample();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 1;
float pitch = 50;
float yaw = 35;
float targetPos[3]={-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
virtual bool wantsTermination()
{
return m_wantsTermination;
}
void enqueueCommand(int command);
};
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
switch (buttonId)
{
case CMD_LOAD_URDF:
case CMD_CREATE_BOX_COLLISION_SHAPE:
case CMD_REQUEST_ACTUAL_STATE:
case CMD_STEP_FORWARD_SIMULATION:
case CMD_SHUTDOWN:
case CMD_SEND_DESIRED_STATE:
case CMD_SEND_BULLET_DATA_STREAM:
{
cl->enqueueCommand(buttonId);
break;
}
default:
{
b3Error("Unknown buttonId");
btAssert(0);
}
};
}
void PhysicsClientExample::enqueueCommand(int command)
{
m_userCommandRequests.push_back(command);
b3Printf("User put command request %d on queue (queue length = %d)\n",command, m_userCommandRequests.size());
}
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_wantsTermination(false)
{
b3Printf("Started PhysicsClientExample\n");
}
PhysicsClientExample::~PhysicsClientExample()
{
b3Printf("~PhysicsClientExample\n");
}
void PhysicsClientExample::createButton(const char* name, int buttonId, bool isTrigger )
{
ButtonParams button(name,buttonId, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
}
void PhysicsClientExample::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
bool isTrigger = false;
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
} else
{
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
}
if (!m_physicsClient.connect())
{
b3Warning("Cannot eonnect to physics client");
}
}
void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_physicsClient.isConnected())
{
m_physicsClient.processServerStatus();
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int command = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
SharedMemoryCommand tmp;
tmp.m_type = command;
m_physicsClient.submitClientCommand(tmp);
}
}
}
}
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options)
{
return new PhysicsClientExample(options.m_guiHelper);
}

6
examples/SharedMemory/PhysicsClientExample.h Normal file
View File

@@ -0,0 +1,6 @@
#ifndef PHYSICS_CLIENT_EXAMPLE_H
#define PHYSICS_CLIENT_EXAMPLE_H
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options);
#endif//PHYSICS_CLIENT_EXAMPLE_H

411
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -1,19 +1,23 @@
#include "PhysicsServer.h" #include "PhysicsServer.h"
#include "PosixSharedMemory.h" #include "PosixSharedMemory.h"
#include "Win32SharedMemory.h" #include "Win32SharedMemory.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h" #include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h" #include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "btBulletDynamicsCommon.h"
#include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h" #include "../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "LinearMath/btSerializer.h"
#include "Bullet3Common/b3Logging.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryCommon.h"
//const char* blaatnaam = "basename";
struct UrdfLinkNameMapUtil struct UrdfLinkNameMapUtil
{ {
btMultiBody* m_mb; btMultiBody* m_mb;
@@ -24,131 +28,137 @@ struct UrdfLinkNameMapUtil
} }
}; };
class PhysicsServer : public SharedMemoryCommon struct PhysicsServerInternalData
{ {
SharedMemoryInterface* m_sharedMemory; SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1; SharedMemoryExampleData* m_testBlock1;
bool m_isConnected;
//btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters; btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper; btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap; btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap;
btAlignedObjectArray<std::string*> m_strings; btAlignedObjectArray<std::string*> m_strings;
bool m_wantsShutdown;
void createJointMotors(btMultiBody* body); btMultiBodyDynamicsWorld* m_dynamicsWorld;
bool supportsJointMotor(btMultiBody* body, int linkIndex); struct GUIHelperInterface* m_guiHelper;
public:
PhysicsServer(GUIHelperInterface* helper); PhysicsServerInternalData()
:m_sharedMemory(0),
virtual ~PhysicsServer(); m_testBlock1(0),
m_isConnected(false),
virtual void initPhysics(); m_dynamicsWorld(0),
m_guiHelper(0)
virtual void stepSimulation(float deltaTime);
void releaseSharedMemory();
bool loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase);
virtual void resetCamera()
{ {
float dist = 5;
float pitch = 50;
float yaw = 35;
float targetPos[3]={0,0,0};//-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
} }
virtual bool wantsTermination();
}; };
PhysicsServer::PhysicsServer(GUIHelperInterface* helper)
:SharedMemoryCommon(helper), PhysicsServerSharedMemory::PhysicsServerSharedMemory()
m_testBlock1(0),
m_wantsShutdown(false)
{ {
b3Printf("Started PhysicsServer\n"); m_data = new PhysicsServerInternalData();
bool useServer = true;
#ifdef _WIN32 #ifdef _WIN32
m_sharedMemory = new Win32SharedMemoryServer(); m_data->m_sharedMemory = new Win32SharedMemoryServer();
#else #else
m_sharedMemory = new PosixSharedMemory(); m_data->m_sharedMemory = new PosixSharedMemory();
#endif #endif
} }
void PhysicsServer::releaseSharedMemory() PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
{
delete m_data;
}
bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper)
{
m_data->m_dynamicsWorld = dynamicsWorld;
m_data->m_guiHelper = guiHelper;
bool allowCreation = true;
m_data->m_testBlock1 = (SharedMemoryExampleData*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlock1)
{
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
if (allowSharedMemoryInitialization)
{
InitSharedMemoryExampleData(m_data->m_testBlock1);
b3Printf("Created and initialized shared memory block");
m_data->m_isConnected = true;
} else
{
b3Error("Error: please start server before client\n");
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
return false;
}
} else
{
b3Printf("Connected to existing shared memory, status OK.\n");
m_data->m_isConnected = true;
}
} else
{
b3Error("Cannot connect to shared memory");
return false;
}
return true;
}
void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
{ {
b3Printf("releaseSharedMemory1\n"); b3Printf("releaseSharedMemory1\n");
if (m_testBlock1) if (m_data->m_testBlock1)
{
b3Printf("m_testBlock1\n");
if (deInitializeSharedMemory)
{
m_data->m_testBlock1->m_magicId = 0;
b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId);
}
btAssert(m_data->m_sharedMemory);
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
}
if (m_data->m_sharedMemory)
{
b3Printf("m_sharedMemory\n");
delete m_data->m_sharedMemory;
m_data->m_sharedMemory = 0;
m_data->m_testBlock1 = 0;
}
}
void PhysicsServerSharedMemory::releaseSharedMemory()
{
b3Printf("releaseSharedMemory1\n");
if (m_data->m_testBlock1)
{ {
b3Printf("m_testBlock1\n"); b3Printf("m_testBlock1\n");
m_testBlock1->m_magicId = 0; m_data->m_testBlock1->m_magicId = 0;
b3Printf("magic id = %d\n",m_testBlock1->m_magicId); b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId);
btAssert(m_sharedMemory); btAssert(m_data->m_sharedMemory);
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
} }
if (m_sharedMemory) if (m_data->m_sharedMemory)
{ {
b3Printf("m_sharedMemory\n"); b3Printf("m_sharedMemory\n");
delete m_sharedMemory; delete m_data->m_sharedMemory;
m_sharedMemory = 0; m_data->m_sharedMemory = 0;
m_testBlock1 = 0; m_data->m_testBlock1 = 0;
}
}
PhysicsServer::~PhysicsServer()
{
releaseSharedMemory();
}
void PhysicsServer::initPhysics()
{
///for this testing we use Z-axis up
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld();
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 grav(0,0,0);
grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav);
m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
// btAssert(m_testBlock1);
if (m_testBlock1)
{
// btAssert(m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER)
{
b3Printf("Warning: shared memory is already initialized, did you already spawn a server?\n");
}
m_testBlock1->m_numClientCommands = 0;
m_testBlock1->m_numServerCommands = 0;
m_testBlock1->m_numProcessedClientCommands=0;
m_testBlock1->m_numProcessedServerCommands=0;
m_testBlock1->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
b3Printf("Shared memory succesfully allocated\n");
} else
{
b3Error("Couldn't allocated shared memory, is it implemented on your operating system?\n");
} }
} }
bool PhysicsServer::wantsTermination() bool PhysicsServerSharedMemory::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
return m_wantsShutdown;
}
bool PhysicsServer::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{ {
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic); ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic);
@@ -157,7 +167,7 @@ bool PhysicsServer::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
} }
//for testing, create joint motors for revolute and prismatic joints //for testing, create joint motors for revolute and prismatic joints
void PhysicsServer::createJointMotors(btMultiBody* mb) void PhysicsServerSharedMemory::createJointMotors(btMultiBody* mb)
{ {
int numLinks = mb->getNumLinks(); int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++) for (int i=0;i<numLinks;i++)
@@ -171,17 +181,27 @@ void PhysicsServer::createJointMotors(btMultiBody* mb)
btScalar desiredVelocity = 0.f; btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0); //motor->setMaxAppliedImpulse(0);
m_multiBodyJointMotorMap.insert(mbLinkIndex,motor); m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,motor);
m_dynamicsWorld->addMultiBodyConstraint(motor); m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);
} }
} }
} }
bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase) bool useMultiBody, bool useFixedBase)
{ {
btAssert(m_data->m_dynamicsWorld);
BulletURDFImporter u2b(m_guiHelper); if (!m_data->m_dynamicsWorld)
{
b3Error("loadUrdf: No valid m_dynamicsWorld");
return false;
}
BulletURDFImporter u2b(m_data->m_guiHelper);
bool loadOk = u2b.loadURDF(fileName); bool loadOk = u2b.loadURDF(fileName);
if (loadOk) if (loadOk)
{ {
@@ -193,9 +213,9 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
tr.setRotation(orn); tr.setRotation(orn);
int rootLinkIndex = u2b.getRootLinkIndex(); int rootLinkIndex = u2b.getRootLinkIndex();
// printf("urdf root link index = %d\n",rootLinkIndex); // printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper); MyMultiBodyCreator creation(m_data->m_guiHelper);
ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
btMultiBody* mb = creation.getBulletMultiBody(); btMultiBody* mb = creation.getBulletMultiBody();
if (useMultiBody) if (useMultiBody)
{ {
@@ -205,9 +225,9 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire //serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil; UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
m_urdfLinkNameMapper.push_back(util); m_data->m_urdfLinkNameMapper.push_back(util);
util->m_mb = mb; util->m_mb = mb;
util->m_memSerializer = new btDefaultSerializer(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE,(unsigned char*)m_testBlock1->m_bulletStreamDataServerToClient); util->m_memSerializer = new btDefaultSerializer(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE,(unsigned char*)m_data->m_testBlock1->m_bulletStreamDataServerToClient);
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them); //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0); util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
@@ -217,18 +237,18 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0); util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
int urdfLinkIndex = creation.m_mb2urdfLink[i]; int urdfLinkIndex = creation.m_mb2urdfLink[i];
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
m_strings.push_back(linkName); m_data->m_strings.push_back(linkName);
util->m_memSerializer->registerNameForPointer(linkName->c_str(),linkName->c_str()); util->m_memSerializer->registerNameForPointer(linkName->c_str(),linkName->c_str());
mb->getLink(i).m_linkName = linkName->c_str(); mb->getLink(i).m_linkName = linkName->c_str();
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str());
m_strings.push_back(jointName); m_data->m_strings.push_back(jointName);
util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str()); util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str());
mb->getLink(i).m_jointName = jointName->c_str(); mb->getLink(i).m_jointName = jointName->c_str();
} }
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_strings.push_back(baseName); m_data->m_strings.push_back(baseName);
util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str()); util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str());
@@ -250,14 +270,17 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
} }
} else } else
{ {
for (int i=0;i<m_dynamicsWorld->getNumConstraints();i++) btAssert(0);
/*
for (int i=0;i<m_data->m_dynamicsWorld->getNumConstraints();i++)
{ {
btTypedConstraint* c = m_dynamicsWorld->getConstraint(i); btTypedConstraint* c = m_data->m_dynamicsWorld->getConstraint(i);
btJointFeedback* fb = new btJointFeedback(); btJointFeedback* fb = new btJointFeedback();
m_jointFeedbacks.push_back(fb); m_data->m_jointFeedbacks.push_back(fb);
c->setJointFeedback(fb); c->setJointFeedback(fb);
} }
*/
return true; return true;
} }
@@ -266,47 +289,44 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
return false; return false;
} }
void PhysicsServer::stepSimulation(float deltaTime)
{
bool wantsShutdown = false; void PhysicsServerSharedMemory::processClientCommands()
{
if (m_testBlock1) if (m_data->m_isConnected && m_data->m_testBlock1)
{ {
///we ignore overflow of integer for now ///we ignore overflow of integer for now
if (m_testBlock1->m_numClientCommands> m_testBlock1->m_numProcessedClientCommands) if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands)
{ {
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
btAssert(m_testBlock1->m_numClientCommands==m_testBlock1->m_numProcessedClientCommands+1); btAssert(m_data->m_testBlock1->m_numClientCommands==m_data->m_testBlock1->m_numProcessedClientCommands+1);
const SharedMemoryCommand& clientCmd =m_testBlock1->m_clientCommands[0]; const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
m_testBlock1->m_numProcessedClientCommands++; m_data->m_testBlock1->m_numProcessedClientCommands++;
//consume the command //consume the command
switch (clientCmd.m_type) switch (clientCmd.m_type)
{ {
case CMD_SEND_BULLET_DATA_STREAM: case CMD_SEND_BULLET_DATA_STREAM:
{ {
b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength);
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_dynamicsWorld); btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
this->m_worldImporters.push_back(worldImporter); m_data->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
if (completedOk) if (completedOk)
{ {
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED; serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED;
} else } else
{ {
serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_FAILED; serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_FAILED;
} }
m_testBlock1->m_numServerCommands++; m_data->m_testBlock1->m_numServerCommands++;
break; break;
} }
case CMD_LOAD_URDF: case CMD_LOAD_URDF:
@@ -324,13 +344,13 @@ void PhysicsServer::stepSimulation(float deltaTime)
urdfArgs.m_initialOrientation[2], urdfArgs.m_initialOrientation[2],
urdfArgs.m_initialOrientation[3]), urdfArgs.m_initialOrientation[3]),
urdfArgs.m_useMultiBody, urdfArgs.m_useFixedBase); urdfArgs.m_useMultiBody, urdfArgs.m_useFixedBase);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
if (completedOk) if (completedOk)
{ {
if (this->m_urdfLinkNameMapper.size()) if (m_data->m_urdfLinkNameMapper.size())
{ {
serverCmd.m_dataStreamArguments.m_streamChunkLength = m_urdfLinkNameMapper.at(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; serverCmd.m_type =CMD_URDF_LOADING_COMPLETED;
} else } else
@@ -338,7 +358,7 @@ void PhysicsServer::stepSimulation(float deltaTime)
serverCmd.m_type =CMD_URDF_LOADING_FAILED; serverCmd.m_type =CMD_URDF_LOADING_FAILED;
} }
m_testBlock1->m_numServerCommands++; m_data->m_testBlock1->m_numServerCommands++;
@@ -352,9 +372,9 @@ void PhysicsServer::stepSimulation(float deltaTime)
//} //}
b3Printf("Processed CMD_SEND_DESIRED_STATE"); b3Printf("Processed CMD_SEND_DESIRED_STATE");
if (m_dynamicsWorld->getNumMultibodies()>0) if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{ {
btMultiBody* mb = m_dynamicsWorld->getMultiBody(0); btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
btAssert(mb); btAssert(mb);
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
@@ -365,12 +385,12 @@ void PhysicsServer::stepSimulation(float deltaTime)
mb->clearForcesAndTorques(); mb->clearForcesAndTorques();
int torqueIndex = 0; int torqueIndex = 0;
btVector3 f(m_testBlock1->m_desiredStateForceTorque[0], btVector3 f(m_data->m_testBlock1->m_desiredStateForceTorque[0],
m_testBlock1->m_desiredStateForceTorque[1], m_data->m_testBlock1->m_desiredStateForceTorque[1],
m_testBlock1->m_desiredStateForceTorque[2]); m_data->m_testBlock1->m_desiredStateForceTorque[2]);
btVector3 t(m_testBlock1->m_desiredStateForceTorque[3], btVector3 t(m_data->m_testBlock1->m_desiredStateForceTorque[3],
m_testBlock1->m_desiredStateForceTorque[4], m_data->m_testBlock1->m_desiredStateForceTorque[4],
m_testBlock1->m_desiredStateForceTorque[5]); m_data->m_testBlock1->m_desiredStateForceTorque[5]);
torqueIndex+=6; torqueIndex+=6;
mb->addBaseForce(f); mb->addBaseForce(f);
mb->addBaseTorque(t); mb->addBaseTorque(t);
@@ -379,7 +399,7 @@ void PhysicsServer::stepSimulation(float deltaTime)
for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++) for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{ {
double torque = m_testBlock1->m_desiredStateForceTorque[torqueIndex]; double torque = m_data->m_testBlock1->m_desiredStateForceTorque[torqueIndex];
mb->addJointTorqueMultiDof(link,dof,torque); mb->addJointTorqueMultiDof(link,dof,torque);
torqueIndex++; torqueIndex++;
} }
@@ -390,25 +410,25 @@ void PhysicsServer::stepSimulation(float deltaTime)
{ {
int numMotors = 0; int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque //find the joint motors and apply the desired velocity and maximum force/torque
if (m_dynamicsWorld->getNumMultibodies()>0) if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{ {
btMultiBody* mb = m_dynamicsWorld->getMultiBody(0); btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base int dofIndex = 6;//skip the 3 linear + 3 angular degree of freedom entries of the base
for (int link=0;link<mb->getNumLinks();link++) for (int link=0;link<mb->getNumLinks();link++)
{ {
if (supportsJointMotor(mb,link)) if (supportsJointMotor(mb,link))
{ {
btMultiBodyJointMotor** motorPtr = m_multiBodyJointMotorMap[link]; btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link];
if (motorPtr) if (motorPtr)
{ {
btMultiBodyJointMotor* motor = *motorPtr; btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = m_testBlock1->m_desiredStateQdot[dofIndex]; btScalar desiredVelocity = m_data->m_testBlock1->m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity); motor->setVelocityTarget(desiredVelocity);
btScalar physicsDeltaTime=1./60.;//todo: set the physicsDeltaTime explicitly in the API, separate from the 'stepSimulation' btScalar physicsDeltaTime=1./60.;//todo: set the physicsDeltaTime explicitly in the API, separate from the 'stepSimulation'
btScalar maxImp = m_testBlock1->m_desiredStateForceTorque[dofIndex]*physicsDeltaTime; btScalar maxImp = m_data->m_testBlock1->m_desiredStateForceTorque[dofIndex]*physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp); motor->setMaxAppliedImpulse(maxImp);
numMotors++; numMotors++;
@@ -431,18 +451,18 @@ void PhysicsServer::stepSimulation(float deltaTime)
SharedMemoryCommand& serverCmd = m_testBlock1->m_serverCommands[0]; SharedMemoryCommand& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
serverCmd.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; serverCmd.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
m_testBlock1->m_numServerCommands++; m_data->m_testBlock1->m_numServerCommands++;
break; break;
} }
case CMD_REQUEST_ACTUAL_STATE: case CMD_REQUEST_ACTUAL_STATE:
{ {
b3Printf("Sending the actual state (Q,U)"); b3Printf("Sending the actual state (Q,U)");
if (m_dynamicsWorld->getNumMultibodies()>0) if (m_data->m_dynamicsWorld->getNumMultibodies()>0)
{ {
btMultiBody* mb = m_dynamicsWorld->getMultiBody(0); btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
SharedMemoryCommand& serverCmd = m_testBlock1->m_serverCommands[0]; SharedMemoryCommand& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
@@ -458,37 +478,37 @@ void PhysicsServer::stepSimulation(float deltaTime)
tr.setRotation(mb->getWorldToBaseRot().inverse()); tr.setRotation(mb->getWorldToBaseRot().inverse());
//base position in world space, carthesian //base position in world space, carthesian
m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0]; m_data->m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0];
m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1]; m_data->m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1];
m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2]; m_data->m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian //base orientation, quaternion x,y,z,w, in world space, carthesian
m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0]; m_data->m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0];
m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1]; m_data->m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1];
m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2]; m_data->m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2];
m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3]; m_data->m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion totalDegreeOfFreedomQ +=7;//pos + quaternion
//base linear velocity (in world space, carthesian) //base linear velocity (in world space, carthesian)
m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0]; m_data->m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0];
m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1]; m_data->m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1];
m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2]; m_data->m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian) //base angular velocity (in world space, carthesian)
m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0]; m_data->m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0];
m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1]; m_data->m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1];
m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2]; m_data->m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
} }
for (int l=0;l<mb->getNumLinks();l++) for (int l=0;l<mb->getNumLinks();l++)
{ {
for (int d=0;d<mb->getLink(l).m_posVarCount;d++) for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
{ {
m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; m_data->m_testBlock1->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
} }
for (int d=0;d<mb->getLink(l).m_dofCount;d++) for (int d=0;d<mb->getLink(l).m_dofCount;d++)
{ {
m_testBlock1->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; m_data->m_testBlock1->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
} }
} }
@@ -523,7 +543,7 @@ void PhysicsServer::stepSimulation(float deltaTime)
} }
*/ */
m_testBlock1->m_numServerCommands++; m_data->m_testBlock1->m_numServerCommands++;
break; break;
@@ -533,18 +553,19 @@ void PhysicsServer::stepSimulation(float deltaTime)
b3Printf("Step simulation request"); b3Printf("Step simulation request");
double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds; double timeStep = clientCmd.m_stepSimulationArguments.m_deltaTimeInSeconds;
m_dynamicsWorld->stepSimulation(timeStep); m_data->m_dynamicsWorld->stepSimulation(timeStep);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_testBlock1->m_numServerCommands++; m_data->m_testBlock1->m_numServerCommands++;
break; break;
} }
case CMD_SHUTDOWN: case CMD_SHUTDOWN:
{ {
wantsShutdown = true; btAssert(0);
//wantsShutdown = true;
break; break;
} }
case CMD_CREATE_BOX_COLLISION_SHAPE: case CMD_CREATE_BOX_COLLISION_SHAPE:
@@ -553,13 +574,18 @@ void PhysicsServer::stepSimulation(float deltaTime)
btTransform startTrans; btTransform startTrans;
startTrans.setIdentity(); startTrans.setIdentity();
startTrans.setOrigin(btVector3(0,0,-4)); startTrans.setOrigin(btVector3(0,0,-4));
btCollisionShape* shape = createBoxShape(halfExtents);
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);
btCollisionShape* shape = worldImporter->createBoxShape(halfExtents);
btScalar mass = 0.f; btScalar mass = 0.f;
createRigidBody(mass,startTrans,shape); bool isDynamic = (mass>0);
this->m_guiHelper->autogenerateGraphicsObjects(this->m_dynamicsWorld); worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0]; m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED; serverCmd.m_type =CMD_STEP_FORWARD_SIMULATION_COMPLETED;
m_testBlock1->m_numServerCommands++; m_data->m_testBlock1->m_numServerCommands++;
break; break;
} }
default: default:
@@ -575,19 +601,4 @@ void PhysicsServer::stepSimulation(float deltaTime)
} }
} }
if (wantsShutdown)
{
b3Printf("releaseSharedMemory!\n");
m_wantsShutdown = true;
releaseSharedMemory();
}
} }
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options)
{
return new PhysicsServer(options.m_guiHelper);
}

36
examples/SharedMemory/PhysicsServer.h
View File

@@ -1,9 +1,35 @@
#ifndef PHYSICS_SERVER_H #ifndef PHYSICS_SERVER_SHARED_MEMORY_H
#define PHYSICS_SERVER_H #define PHYSICS_SERVER_SHARED_MEMORY_H
class PhysicsServerSharedMemory
{
struct PhysicsServerInternalData* m_data;
protected:
void createJointMotors(class btMultiBody* body);
void releaseSharedMemory();
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
bool useMultiBody, bool useFixedBase);
public:
PhysicsServerSharedMemory();
virtual ~PhysicsServerSharedMemory();
//todo: implement option to allocated shared memory from client
virtual bool connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper);
virtual void disconnectSharedMemory (bool deInitializeSharedMemory);
virtual void processClientCommands();
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
};
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options); #endif //PHYSICS_SERVER_EXAMPLESHARED_MEMORY_H
#endif //PHYSICS_SERVER_H

110
examples/SharedMemory/PhysicsServerExample.cpp Normal file
View File

@@ -0,0 +1,110 @@
#include "PhysicsServerExample.h"
#include "PhysicsServer.h"
#include "SharedMemoryCommon.h"
//const char* blaatnaam = "basename";
struct UrdfLinkNameMapUtil
{
btMultiBody* m_mb;
btDefaultSerializer* m_memSerializer;
UrdfLinkNameMapUtil():m_mb(0),m_memSerializer(0)
{
}
};
class PhysicsServerExample : public SharedMemoryCommon
{
PhysicsServerSharedMemory m_physicsServer;
bool m_wantsShutdown;
public:
PhysicsServerExample(GUIHelperInterface* helper);
virtual ~PhysicsServerExample();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
bool loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase);
virtual void resetCamera()
{
float dist = 5;
float pitch = 50;
float yaw = 35;
float targetPos[3]={0,0,0};//-3,2.8,-2.5};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
virtual bool wantsTermination();
};
PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_wantsShutdown(false)
{
b3Printf("Started PhysicsServer\n");
bool useServer = true;
}
PhysicsServerExample::~PhysicsServerExample()
{
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
void PhysicsServerExample::initPhysics()
{
///for this testing we use Z-axis up
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld();
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 grav(0,0,0);
grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav);
bool allowSharedMemoryInitialization = true;
m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper);
}
bool PhysicsServerExample::wantsTermination()
{
return m_wantsShutdown;
}
void PhysicsServerExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
}
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options)
{
return new PhysicsServerExample(options.m_guiHelper);
}

9
examples/SharedMemory/PhysicsServerExample.h Normal file
View File

@@ -0,0 +1,9 @@
#ifndef PHYSICS_SERVER_EXAMPLE_H
#define PHYSICS_SERVER_EXAMPLE_H
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options);
#endif //PHYSICS_SERVER_EXAMPLE_H

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -123,5 +123,6 @@ struct SharedMemoryCommand
}; };
}; };
typedef SharedMemoryCommand ServerStatus;
#endif //SHARED_MEMORY_COMMANDS_H #endif //SHARED_MEMORY_COMMANDS_H

12
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -45,6 +45,16 @@ struct SharedMemoryExampleData
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
}; };
inline void InitSharedMemoryExampleData(SharedMemoryExampleData* sharedMemoryBlock)
{
sharedMemoryBlock->m_numClientCommands = 0;
sharedMemoryBlock->m_numServerCommands = 0;
sharedMemoryBlock->m_numProcessedClientCommands=0;
sharedMemoryBlock->m_numProcessedServerCommands=0;
sharedMemoryBlock->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
}
#define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData) #define SHARED_MEMORY_SIZE sizeof(SharedMemoryExampleData)
@@ -57,7 +67,7 @@ class SharedMemoryInterface
{ {
} }
virtual void* allocateSharedMemory(int key, int size) =0; virtual void* allocateSharedMemory(int key, int size, bool allowCreation) =0;
virtual void releaseSharedMemory(int key, int size) =0; virtual void releaseSharedMemory(int key, int size) =0;
}; };
#endif #endif

31
examples/SharedMemory/Win32SharedMemory.cpp
View File

@@ -32,35 +32,34 @@ Win32SharedMemory::~Win32SharedMemory()
delete m_internalData; delete m_internalData;
} }
void* Win32SharedMemory::allocateSharedMemory(int key, int size) void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
{ {
b3Assert(m_internalData->m_buf==0); b3Assert(m_internalData->m_buf==0);
m_internalData->m_hMapFile = OpenFileMapping(
if (this->isServer()) FILE_MAP_ALL_ACCESS, // read/write access
FALSE, // do not inherit the name
szName); // name of mapping object
if (m_internalData->m_hMapFile==NULL)
{ {
m_internalData->m_hMapFile = CreateFileMapping( if (allowCreation)
{
m_internalData->m_hMapFile = CreateFileMapping(
INVALID_HANDLE_VALUE, // use paging file INVALID_HANDLE_VALUE, // use paging file
NULL, // default security NULL, // default security
PAGE_READWRITE, // read/write access PAGE_READWRITE, // read/write access
0, // maximum object size (high-order DWORD) 0, // maximum object size (high-order DWORD)
size, // maximum object size (low-order DWORD) size, // maximum object size (low-order DWORD)
szName); // name of mapping object szName); // name of mapping object
} else } else
{ {
m_internalData->m_hMapFile = OpenFileMapping( b3Error("Could not create file mapping object (%d).\n",GetLastError());
FILE_MAP_ALL_ACCESS, // read/write access return 0;
FALSE, // do not inherit the name }
szName); // name of mapping object
} }
if (m_internalData->m_hMapFile == NULL)
{
b3Error("Could not create file mapping object (%d).\n",GetLastError());
return 0;
}
m_internalData->m_buf = MapViewOfFile(m_internalData->m_hMapFile, // handle to map object m_internalData->m_buf = MapViewOfFile(m_internalData->m_hMapFile, // handle to map object
FILE_MAP_ALL_ACCESS, // read/write permission FILE_MAP_ALL_ACCESS, // read/write permission
0, 0,

14
examples/SharedMemory/Win32SharedMemory.h
View File

@@ -14,9 +14,9 @@ public:
Win32SharedMemory(); Win32SharedMemory();
virtual ~Win32SharedMemory(); virtual ~Win32SharedMemory();
virtual void* allocateSharedMemory(int key, int size); virtual void* allocateSharedMemory(int key, int size, bool allowCreation);
virtual void releaseSharedMemory(int key, int size); virtual void releaseSharedMemory(int key, int size);
virtual bool isServer() const = 0;
}; };
class Win32SharedMemoryServer : public Win32SharedMemory class Win32SharedMemoryServer : public Win32SharedMemory
@@ -24,10 +24,7 @@ class Win32SharedMemoryServer : public Win32SharedMemory
public: public:
Win32SharedMemoryServer(); Win32SharedMemoryServer();
virtual ~Win32SharedMemoryServer(); virtual ~Win32SharedMemoryServer();
virtual bool isServer() const
{
return true;
}
}; };
class Win32SharedMemoryClient : public Win32SharedMemory class Win32SharedMemoryClient : public Win32SharedMemory
@@ -35,10 +32,7 @@ class Win32SharedMemoryClient : public Win32SharedMemory
public: public:
Win32SharedMemoryClient(); Win32SharedMemoryClient();
virtual ~Win32SharedMemoryClient(); virtual ~Win32SharedMemoryClient();
virtual bool isServer() const
{
return false;
}
}; };

4
examples/SharedMemory/main.cpp
View File

@@ -14,8 +14,8 @@ subject to the following restrictions:
*/ */
#include "PhysicsServer.h" #include "PhysicsServerExample.h"
#include "PhysicsClient.h" #include "PhysicsClientExample.h"
#include "Bullet3Common/b3CommandLineArgs.h" #include "Bullet3Common/b3CommandLineArgs.h"
#include "../CommonInterfaces/CommonExampleInterface.h" #include "../CommonInterfaces/CommonExampleInterface.h"