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

359
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -1,179 +1,113 @@
#include "PhysicsClient.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "PosixSharedMemory.h"
#include "Win32SharedMemory.h"
#include "SharedMemoryCommon.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "Bullet3Common/b3Logging.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;
SharedMemoryExampleData* m_testBlock1;
int m_counter;
bool m_wantsTermination;
btAlignedObjectArray<int> m_userCommandRequests;
int m_counter;
bool m_serverLoadUrdfOK;
bool m_isConnected;
bool m_waitingForServer;
void processServerCommands();
void createClientCommand();
void createButton(const char* name, int id, bool isTrigger );
public:
PhysicsClient(GUIHelperInterface* helper);
virtual ~PhysicsClient();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
PhysicsClientSharedMemoryInternalData()
:m_sharedMemory(0),
m_testBlock1(0),
m_counter(0),
m_serverLoadUrdfOK(false),
m_isConnected(false),
m_waitingForServer(false)
{
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;
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;
}
m_data = new PhysicsClientSharedMemoryInternalData;
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
m_sharedMemory = new Win32SharedMemoryClient();
m_data->m_sharedMemory = new Win32SharedMemoryClient();
#else
m_sharedMemory = new PosixSharedMemory();
m_data->m_sharedMemory = new PosixSharedMemory();
#endif
}
PhysicsClient::~PhysicsClient()
PhysicsClientSharedMemory::~PhysicsClientSharedMemory()
{
b3Printf("~PhysicsClient\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
delete m_sharedMemory;
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
delete m_data->m_sharedMemory;
delete m_data;
}
void PhysicsClient::createButton(const char* name, int buttonId, bool isTrigger )
bool PhysicsClientSharedMemory::isConnected() const
{
ButtonParams button(name,buttonId, isTrigger);
button.m_callback = MyCallback;
button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button);
return m_data->m_isConnected ;
}
void PhysicsClient::initPhysics()
bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
{
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);
}
m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
if (m_testBlock1)
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)
{
// btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
b3Error("Error: please start server before client\n");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_testBlock1 = 0;
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("Shared Memory status is OK\n");
b3Printf("Connected to existing shared memory, status OK.\n");
m_data->m_isConnected = true;
}
} 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
switch (serverCmd.m_type)
@@ -181,12 +115,12 @@ void PhysicsClient::processServerCommands()
case CMD_URDF_LOADING_COMPLETED:
{
m_serverLoadUrdfOK = true;
m_data->m_serverLoadUrdfOK = true;
b3Printf("Server loading the URDF OK\n");
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->parse(false);
for (int i=0;i<bf->m_multiBodies.size();i++)
@@ -195,7 +129,7 @@ void PhysicsClient::processServerCommands()
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)
{
b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
@@ -213,7 +147,7 @@ void PhysicsClient::processServerCommands()
}
} else
{
btMultiBodyFloatData* mb = (btMultiBodyFloatData*) bf->m_multiBodies[i];
Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i];
if (mb->m_baseName)
{
b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
@@ -247,7 +181,7 @@ void PhysicsClient::processServerCommands()
case CMD_URDF_LOADING_FAILED:
{
b3Printf("Server failed loading the URDF...\n");
m_serverLoadUrdfOK = false;
m_data->m_serverLoadUrdfOK = false;
break;
}
@@ -272,8 +206,8 @@ void PhysicsClient::processServerCommands()
{
b3Printf("Received actual state\n");
int numQ = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
int numQ = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("size Q = %d, size U = %d\n", numQ,numU);
char msg[1024];
@@ -283,10 +217,10 @@ void PhysicsClient::processServerCommands()
{
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
{
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);
@@ -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)
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
{
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
if (m_userCommandRequests.size())
//process command
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
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_data->m_waitingForServer = true;
m_userCommandRequests.pop_back();
m_waitingForServer = true;
switch (command)
switch (command.m_type)
{
case CMD_LOAD_URDF:
{
if (!m_serverLoadUrdfOK)
if (!m_data->m_serverLoadUrdfOK)
{
m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
sprintf(m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
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_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0;
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_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0;
m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0;
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_type =CMD_LOAD_URDF;
sprintf(m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[0] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[1] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[0] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[1] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
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");
} else
{
@@ -367,11 +293,11 @@ void PhysicsClient::createClientCommand()
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
if (m_serverLoadUrdfOK)
if (m_data->m_serverLoadUrdfOK)
{
b3Printf("Requesting create box collision shape\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
m_testBlock1->m_numClientCommands++;
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");
@@ -403,13 +329,13 @@ void PhysicsClient::createClientCommand()
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);
m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_BULLET_DATA_STREAM;
m_testBlock1->m_clientCommands[0].m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_testBlock1->m_numClientCommands++;
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
{
@@ -428,11 +354,11 @@ void PhysicsClient::createClientCommand()
}
case CMD_REQUEST_ACTUAL_STATE:
{
if (m_serverLoadUrdfOK)
if (m_data->m_serverLoadUrdfOK)
{
b3Printf("Requesting actual state\n");
m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
m_testBlock1->m_numClientCommands++;
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
m_data->m_testBlock1->m_numClientCommands++;
} else
{
@@ -442,10 +368,10 @@ void PhysicsClient::createClientCommand()
}
case CMD_SEND_DESIRED_STATE:
{
if (m_serverLoadUrdfOK)
if (m_data->m_serverLoadUrdfOK)
{
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
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
@@ -453,20 +379,20 @@ void PhysicsClient::createClientCommand()
{
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++)
{
m_testBlock1->m_desiredStateQdot[i] = 1;
m_testBlock1->m_desiredStateForceTorque[i] = 100;
m_data->m_testBlock1->m_desiredStateQdot[i] = 1;
m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
}
break;
}
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++)
{
m_testBlock1->m_desiredStateForceTorque[i] = 100;
m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
}
break;
}
@@ -477,7 +403,7 @@ void PhysicsClient::createClientCommand()
}
}
m_testBlock1->m_numClientCommands++;
m_data->m_testBlock1->m_numClientCommands++;
} else
{
@@ -487,13 +413,13 @@ void PhysicsClient::createClientCommand()
}
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_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
m_testBlock1->m_numClientCommands++;
b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_counter++);
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");
@@ -502,10 +428,10 @@ void PhysicsClient::createClientCommand()
}
case CMD_SHUTDOWN:
{
m_wantsTermination = true;
m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
m_testBlock1->m_numClientCommands++;
m_serverLoadUrdfOK = false;
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;
}
@@ -516,28 +442,7 @@ void PhysicsClient::createClientCommand()
}
}
}
}
void PhysicsClient::stepSimulation(float deltaTime)
{
// btAssert(m_testBlock1);
if (m_testBlock1)
{
processServerCommands();
if (!m_waitingForServer)
{
createClientCommand();
}
}
return true;
}
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
#define PHYSICS_CLIENT_H
#ifndef BT_PHYSICS_CLIENT_API_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 "PosixSharedMemory.h"
#include "Win32SharedMemory.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.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 "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
{
btMultiBody* m_mb;
@@ -24,131 +28,137 @@ struct UrdfLinkNameMapUtil
}
};
class PhysicsServer : public SharedMemoryCommon
struct PhysicsServerInternalData
{
SharedMemoryInterface* m_sharedMemory;
SharedMemoryExampleData* m_testBlock1;
bool m_isConnected;
//btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
btAlignedObjectArray<btJointFeedback*> m_jointFeedbacks;
btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap;
btAlignedObjectArray<std::string*> m_strings;
bool m_wantsShutdown;
void createJointMotors(btMultiBody* body);
bool supportsJointMotor(btMultiBody* body, int linkIndex);
public:
PhysicsServer(GUIHelperInterface* helper);
virtual ~PhysicsServer();
virtual void initPhysics();
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()
btMultiBodyDynamicsWorld* m_dynamicsWorld;
struct GUIHelperInterface* m_guiHelper;
PhysicsServerInternalData()
:m_sharedMemory(0),
m_testBlock1(0),
m_isConnected(false),
m_dynamicsWorld(0),
m_guiHelper(0)
{
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),
m_testBlock1(0),
m_wantsShutdown(false)
PhysicsServerSharedMemory::PhysicsServerSharedMemory()
{
b3Printf("Started PhysicsServer\n");
bool useServer = true;
#ifdef _WIN32
m_sharedMemory = new Win32SharedMemoryServer();
#else
m_sharedMemory = new PosixSharedMemory();
#endif
m_data = new PhysicsServerInternalData();
#ifdef _WIN32
m_data->m_sharedMemory = new Win32SharedMemoryServer();
#else
m_data->m_sharedMemory = new PosixSharedMemory();
#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");
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");
m_testBlock1->m_magicId = 0;
b3Printf("magic id = %d\n",m_testBlock1->m_magicId);
btAssert(m_sharedMemory);
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_data->m_testBlock1->m_magicId = 0;
b3Printf("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_sharedMemory)
if (m_data->m_sharedMemory)
{
b3Printf("m_sharedMemory\n");
delete m_sharedMemory;
m_sharedMemory = 0;
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");
delete m_data->m_sharedMemory;
m_data->m_sharedMemory = 0;
m_data->m_testBlock1 = 0;
}
}
bool PhysicsServer::wantsTermination()
{
return m_wantsShutdown;
}
bool PhysicsServer::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
bool PhysicsServerSharedMemory::supportsJointMotor(btMultiBody* mb, int mbLinkIndex)
{
bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||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
void PhysicsServer::createJointMotors(btMultiBody* mb)
void PhysicsServerSharedMemory::createJointMotors(btMultiBody* mb)
{
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
@@ -171,17 +181,27 @@ void PhysicsServer::createJointMotors(btMultiBody* mb)
btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_multiBodyJointMotorMap.insert(mbLinkIndex,motor);
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_multiBodyJointMotorMap.insert(mbLinkIndex,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)
{
BulletURDFImporter u2b(m_guiHelper);
btAssert(m_data->m_dynamicsWorld);
if (!m_data->m_dynamicsWorld)
{
b3Error("loadUrdf: No valid m_dynamicsWorld");
return false;
}
BulletURDFImporter u2b(m_data->m_guiHelper);
bool loadOk = u2b.loadURDF(fileName);
if (loadOk)
{
@@ -193,9 +213,9 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
tr.setRotation(orn);
int rootLinkIndex = u2b.getRootLinkIndex();
// 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();
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
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
m_urdfLinkNameMapper.push_back(util);
m_data->m_urdfLinkNameMapper.push_back(util);
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);
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);
int urdfLinkIndex = creation.m_mb2urdfLink[i];
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());
mb->getLink(i).m_linkName = linkName->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());
mb->getLink(i).m_jointName = jointName->c_str();
}
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());
@@ -250,14 +270,17 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
}
} 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();
m_jointFeedbacks.push_back(fb);
m_data->m_jointFeedbacks.push_back(fb);
c->setJointFeedback(fb);
}
*/
return true;
}
@@ -266,47 +289,44 @@ bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const b
return false;
}
void PhysicsServer::stepSimulation(float deltaTime)
{
bool wantsShutdown = false;
if (m_testBlock1)
void PhysicsServerSharedMemory::processClientCommands()
{
if (m_data->m_isConnected && m_data->m_testBlock1)
{
///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
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];
m_testBlock1->m_numProcessedClientCommands++;
const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
m_data->m_testBlock1->m_numProcessedClientCommands++;
//consume the command
switch (clientCmd.m_type)
switch (clientCmd.m_type)
{
case CMD_SEND_BULLET_DATA_STREAM:
{
b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength);
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_dynamicsWorld);
this->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
SharedMemoryCommand& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
if (completedOk)
{
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED;
} else
{
serverCmd.m_type =CMD_BULLET_DATA_STREAM_RECEIVED_FAILED;
}
m_testBlock1->m_numServerCommands++;
m_data->m_testBlock1->m_numServerCommands++;
break;
}
case CMD_LOAD_URDF:
@@ -324,13 +344,13 @@ void PhysicsServer::stepSimulation(float deltaTime)
urdfArgs.m_initialOrientation[2],
urdfArgs.m_initialOrientation[3]),
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 (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;
} else
@@ -338,7 +358,7 @@ void PhysicsServer::stepSimulation(float deltaTime)
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");
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);
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
@@ -365,12 +385,12 @@ void PhysicsServer::stepSimulation(float deltaTime)
mb->clearForcesAndTorques();
int torqueIndex = 0;
btVector3 f(m_testBlock1->m_desiredStateForceTorque[0],
m_testBlock1->m_desiredStateForceTorque[1],
m_testBlock1->m_desiredStateForceTorque[2]);
btVector3 t(m_testBlock1->m_desiredStateForceTorque[3],
m_testBlock1->m_desiredStateForceTorque[4],
m_testBlock1->m_desiredStateForceTorque[5]);
btVector3 f(m_data->m_testBlock1->m_desiredStateForceTorque[0],
m_data->m_testBlock1->m_desiredStateForceTorque[1],
m_data->m_testBlock1->m_desiredStateForceTorque[2]);
btVector3 t(m_data->m_testBlock1->m_desiredStateForceTorque[3],
m_data->m_testBlock1->m_desiredStateForceTorque[4],
m_data->m_testBlock1->m_desiredStateForceTorque[5]);
torqueIndex+=6;
mb->addBaseForce(f);
mb->addBaseTorque(t);
@@ -379,7 +399,7 @@ void PhysicsServer::stepSimulation(float deltaTime)
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);
torqueIndex++;
}
@@ -390,25 +410,25 @@ void PhysicsServer::stepSimulation(float deltaTime)
{
int numMotors = 0;
//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
for (int link=0;link<mb->getNumLinks();link++)
{
if (supportsJointMotor(mb,link))
{
btMultiBodyJointMotor** motorPtr = m_multiBodyJointMotorMap[link];
btMultiBodyJointMotor** motorPtr = m_data->m_multiBodyJointMotorMap[link];
if (motorPtr)
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = m_testBlock1->m_desiredStateQdot[dofIndex];
btScalar desiredVelocity = m_data->m_testBlock1->m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity);
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);
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;
m_testBlock1->m_numServerCommands++;
m_data->m_testBlock1->m_numServerCommands++;
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
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);
SharedMemoryCommand& serverCmd = m_testBlock1->m_serverCommands[0];
btMultiBody* mb = m_data->m_dynamicsWorld->getMultiBody(0);
SharedMemoryCommand& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = 0;
@@ -458,37 +478,37 @@ void PhysicsServer::stepSimulation(float deltaTime)
tr.setRotation(mb->getWorldToBaseRot().inverse());
//base position in world space, carthesian
m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0];
m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1];
m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2];
m_data->m_testBlock1->m_actualStateQ[0] = tr.getOrigin()[0];
m_data->m_testBlock1->m_actualStateQ[1] = tr.getOrigin()[1];
m_data->m_testBlock1->m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0];
m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1];
m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2];
m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3];
m_data->m_testBlock1->m_actualStateQ[3] = tr.getRotation()[0];
m_data->m_testBlock1->m_actualStateQ[4] = tr.getRotation()[1];
m_data->m_testBlock1->m_actualStateQ[5] = tr.getRotation()[2];
m_data->m_testBlock1->m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ +=7;//pos + quaternion
//base linear velocity (in world space, carthesian)
m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0];
m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1];
m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2];
m_data->m_testBlock1->m_actualStateQdot[0] = mb->getBaseVel()[0];
m_data->m_testBlock1->m_actualStateQdot[1] = mb->getBaseVel()[1];
m_data->m_testBlock1->m_actualStateQdot[2] = mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian)
m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0];
m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1];
m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2];
m_data->m_testBlock1->m_actualStateQdot[3] = mb->getBaseOmega()[0];
m_data->m_testBlock1->m_actualStateQdot[4] = mb->getBaseOmega()[1];
m_data->m_testBlock1->m_actualStateQdot[5] = mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF
}
for (int l=0;l<mb->getNumLinks();l++)
{
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++)
{
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;
@@ -533,18 +553,19 @@ void PhysicsServer::stepSimulation(float deltaTime)
b3Printf("Step simulation request");
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;
m_testBlock1->m_numServerCommands++;
m_data->m_testBlock1->m_numServerCommands++;
break;
}
case CMD_SHUTDOWN:
{
wantsShutdown = true;
btAssert(0);
//wantsShutdown = true;
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
@@ -553,13 +574,18 @@ void PhysicsServer::stepSimulation(float deltaTime)
btTransform startTrans;
startTrans.setIdentity();
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;
createRigidBody(mass,startTrans,shape);
this->m_guiHelper->autogenerateGraphicsObjects(this->m_dynamicsWorld);
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
bool isDynamic = (mass>0);
worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,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;
m_testBlock1->m_numServerCommands++;
m_data->m_testBlock1->m_numServerCommands++;
break;
}
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
#define PHYSICS_SERVER_H
#ifndef PHYSICS_SERVER_SHARED_MEMORY_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_H
#endif //PHYSICS_SERVER_EXAMPLESHARED_MEMORY_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

12
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -45,6 +45,16 @@ struct SharedMemoryExampleData
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)
@@ -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;
};
#endif

31
examples/SharedMemory/Win32SharedMemory.cpp
View File

@@ -32,35 +32,34 @@ Win32SharedMemory::~Win32SharedMemory()
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);
if (this->isServer())
m_internalData->m_hMapFile = OpenFileMapping(
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
NULL, // default security
PAGE_READWRITE, // read/write access
0, // maximum object size (high-order DWORD)
size, // maximum object size (low-order DWORD)
szName); // name of mapping object
} else
{
m_internalData->m_hMapFile = OpenFileMapping(
FILE_MAP_ALL_ACCESS, // read/write access
FALSE, // do not inherit the name
szName); // name of mapping object
} else
{
b3Error("Could not create file mapping object (%d).\n",GetLastError());
return 0;
}
}
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
FILE_MAP_ALL_ACCESS, // read/write permission
0,

14
examples/SharedMemory/Win32SharedMemory.h
View File

@@ -14,9 +14,9 @@ public:
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 bool isServer() const = 0;
};
class Win32SharedMemoryServer : public Win32SharedMemory
@@ -24,10 +24,7 @@ class Win32SharedMemoryServer : public Win32SharedMemory
public:
Win32SharedMemoryServer();
virtual ~Win32SharedMemoryServer();
virtual bool isServer() const
{
return true;
}
};
class Win32SharedMemoryClient : public Win32SharedMemory
@@ -35,10 +32,7 @@ class Win32SharedMemoryClient : public Win32SharedMemory
public:
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 "PhysicsClient.h"
#include "PhysicsServerExample.h"
#include "PhysicsClientExample.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "../CommonInterfaces/CommonExampleInterface.h"