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);
}