add command-line argument for example browser and shared memory app, --shared_memory_key=<int>

fix some shared memory issues, client uses attach/detach, server uses create/remove shared memory
implement CMD_RESET_SIMULATION
This commit is contained in:
Erwin Coumans
2015-08-07 00:13:26 -07:00
parent f750275cf9
commit 03d991c92b
14 changed files with 373 additions and 56 deletions

View File

@@ -188,6 +188,22 @@ void GwenParameterInterface::syncParameters()
void GwenParameterInterface::removeAllParameters() void GwenParameterInterface::removeAllParameters()
{ {
for (int i=0;i<m_paramInternalData->m_buttons.size();i++)
{
delete m_paramInternalData->m_buttons[i];
}
m_paramInternalData->m_buttons.clear();
for (int i=0;i<m_paramInternalData->m_buttonEventHandlers.size();i++)
{
delete m_paramInternalData->m_buttonEventHandlers[i];
}
m_paramInternalData->m_buttonEventHandlers.clear();
m_gwenInternalData->m_curYposition+=22;
for (int i=0;i<m_paramInternalData->m_sliders.size();i++) for (int i=0;i<m_paramInternalData->m_sliders.size();i++)
{ {
delete m_paramInternalData->m_sliders[i]; delete m_paramInternalData->m_sliders[i];

View File

@@ -76,6 +76,9 @@ static bool pauseSimulation=false;
int midiBaseIndex = 176; int midiBaseIndex = 176;
extern bool gDisableDeactivation; extern bool gDisableDeactivation;
int gSharedMemoryKey=-1;
///some quick test variable for the OpenCL examples ///some quick test variable for the OpenCL examples
int gPreferredOpenCLDeviceIndex=-1; int gPreferredOpenCLDeviceIndex=-1;
@@ -739,6 +742,8 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
} }
args.GetCmdLineArgument("shared_memory_key", gSharedMemoryKey);
float red,green,blue; float red,green,blue;
s_app->getBackgroundColor(&red,&green,&blue); s_app->getBackgroundColor(&red,&green,&blue);
args.GetCmdLineArgument("background_color_red",red); args.GetCmdLineArgument("background_color_red",red);

View File

@@ -55,7 +55,7 @@ public:
void InitShaders(); void InitShaders();
void CleanupShaders(); void CleanupShaders();
void removeAllInstances(); virtual void removeAllInstances();
virtual void updateShape(int shapeIndex, const float* vertices); virtual void updateShape(int shapeIndex, const float* vertices);

View File

@@ -24,6 +24,8 @@ struct PhysicsClientSharedMemoryInternalData
btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData; btAlignedObjectArray<bParse::btBulletFile*> m_robotMultiBodyData;
btAlignedObjectArray<b3JointInfo> m_jointInfo; btAlignedObjectArray<b3JointInfo> m_jointInfo;
int m_sharedMemoryKey;
int m_counter; int m_counter;
bool m_serverLoadUrdfOK; bool m_serverLoadUrdfOK;
bool m_isConnected; bool m_isConnected;
@@ -37,7 +39,8 @@ struct PhysicsClientSharedMemoryInternalData
m_serverLoadUrdfOK(false), m_serverLoadUrdfOK(false),
m_isConnected(false), m_isConnected(false),
m_waitingForServer(false), m_waitingForServer(false),
m_hasLastServerStatus(false) m_hasLastServerStatus(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY)
{ {
} }
@@ -85,11 +88,16 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory()
delete m_data; delete m_data;
} }
void PhysicsClientSharedMemory::setSharedMemoryKey(int key)
{
m_data->m_sharedMemoryKey = key;
}
void PhysicsClientSharedMemory::disconnectSharedMemory () void PhysicsClientSharedMemory::disconnectSharedMemory ()
{ {
if (m_data->m_isConnected) if (m_data->m_isConnected)
{ {
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
m_data->m_isConnected = false; m_data->m_isConnected = false;
} }
} }
@@ -103,14 +111,14 @@ bool PhysicsClientSharedMemory::connect()
{ {
///server always has to create and initialize shared memory ///server always has to create and initialize shared memory
bool allowCreation = false; bool allowCreation = false;
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation); m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlock1) if (m_data->m_testBlock1)
{ {
if (m_data->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"); b3Error("Error: please start server before client\n");
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0; m_data->m_testBlock1 = 0;
return false; return false;
} else } else
@@ -389,6 +397,18 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
if (!m_data->m_waitingForServer) if (!m_data->m_waitingForServer)
{ {
//a reset simulation command needs special attention, cleanup state
if (command.m_type==CMD_RESET_SIMULATION)
{
for (int i=0;i<m_data->m_robotMultiBodyData.size();i++)
{
delete m_data->m_robotMultiBodyData[i];
}
m_data->m_robotMultiBodyData.clear();
m_data->m_jointInfo.clear();
}
m_data->m_testBlock1->m_clientCommands[0] = command; m_data->m_testBlock1->m_clientCommands[0] = command;
m_data->m_testBlock1->m_numClientCommands++; m_data->m_testBlock1->m_numClientCommands++;
m_data->m_waitingForServer = true; m_data->m_waitingForServer = true;

View File

@@ -32,6 +32,7 @@ public:
virtual void getJointInfo(int index, b3JointInfo& info) const; virtual void getJointInfo(int index, b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);
}; };
#endif //BT_PHYSICS_CLIENT_API_H #endif //BT_PHYSICS_CLIENT_API_H

View File

@@ -29,6 +29,8 @@ protected:
void createButton(const char* name, int id, bool isTrigger ); void createButton(const char* name, int id, bool isTrigger );
void createButtons();
public: public:
//@todo, add accessor methods //@todo, add accessor methods
@@ -64,6 +66,18 @@ public:
void enqueueCommand(const SharedMemoryCommand& orgCommand); void enqueueCommand(const SharedMemoryCommand& orgCommand);
virtual void exitPhysics(){};
virtual void renderScene(){}
virtual void physicsDebugDraw(int debugFlags){}
virtual bool mouseMoveCallback(float x,float y){return false;};
virtual bool mouseButtonCallback(int button, int state, float x, float y){return false;}
virtual bool keyboardCallback(int key, int state){return false;}
virtual void setSharedMemoryKey(int key)
{
m_physicsClient.setSharedMemoryKey(key);
}
}; };
@@ -97,6 +111,11 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
case CMD_CREATE_BOX_COLLISION_SHAPE: case CMD_CREATE_BOX_COLLISION_SHAPE:
{ {
command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE; command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
cl->enqueueCommand(command); cl->enqueueCommand(command);
break; break;
} }
@@ -115,6 +134,7 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
case CMD_SEND_DESIRED_STATE: case CMD_SEND_DESIRED_STATE:
{ {
command.m_type =CMD_SEND_DESIRED_STATE; command.m_type =CMD_SEND_DESIRED_STATE;
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
@@ -141,6 +161,12 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
cl->enqueueCommand(command); cl->enqueueCommand(command);
break; break;
} }
case CMD_RESET_SIMULATION:
{
command.m_type = CMD_RESET_SIMULATION;
cl->enqueueCommand(command);
break;
}
case CMD_SEND_BULLET_DATA_STREAM: case CMD_SEND_BULLET_DATA_STREAM:
{ {
command.m_type = buttonId; command.m_type = buttonId;
@@ -167,7 +193,8 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper) PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper), :SharedMemoryCommon(helper),
m_wantsTermination(false) m_wantsTermination(false),
m_numMotors(0)
{ {
b3Printf("Started PhysicsClientExample\n"); b3Printf("Started PhysicsClientExample\n");
} }
@@ -184,18 +211,26 @@ void PhysicsClientExample::createButton(const char* name, int buttonId, bool isT
button.m_userPointer = this; button.m_userPointer = this;
m_guiHelper->getParameterInterface()->registerButtonParameter(button); m_guiHelper->getParameterInterface()->registerButtonParameter(button);
} }
void PhysicsClientExample::createButtons()
{
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);
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
}
void PhysicsClientExample::initPhysics() void PhysicsClientExample::initPhysics()
{ {
if (m_guiHelper && m_guiHelper->getParameterInterface()) if (m_guiHelper && m_guiHelper->getParameterInterface())
{ {
bool isTrigger = false; createButtons();
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 } else
{ {
@@ -285,6 +320,14 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
m_userCommandRequests.pop_back(); m_userCommandRequests.pop_back();
//for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
if (command.m_type==CMD_RESET_SIMULATION)
{
m_guiHelper->getParameterInterface()->removeAllParameters();
m_numMotors=0;
createButtons();
}
m_physicsClient.submitClientCommand(command); m_physicsClient.submitClientCommand(command);
} }
} }
@@ -293,8 +336,15 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
} }
extern int gSharedMemoryKey;
class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options) class CommonExampleInterface* PhysicsClientCreateFunc(struct CommonExampleOptions& options)
{ {
return new PhysicsClientExample(options.m_guiHelper); PhysicsClientExample* example = new PhysicsClientExample(options.m_guiHelper);
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
}
return example;
} }

View File

@@ -10,8 +10,11 @@
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "btBulletDynamicsCommon.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 "LinearMath/btSerializer.h"
@@ -42,8 +45,14 @@ struct PhysicsServerInternalData
btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap; btHashMap<btHashInt, btMultiBodyJointMotor*> m_multiBodyJointMotorMap;
btAlignedObjectArray<std::string*> m_strings; btAlignedObjectArray<std::string*> m_strings;
btAlignedObjectArray<btCollisionShape*> m_collisionShapes;
btBroadphaseInterface* m_broadphase;
btCollisionDispatcher* m_dispatcher;
btMultiBodyConstraintSolver* m_solver;
btDefaultCollisionConfiguration* m_collisionConfiguration;
btMultiBodyDynamicsWorld* m_dynamicsWorld; btMultiBodyDynamicsWorld* m_dynamicsWorld;
struct GUIHelperInterface* m_guiHelper; struct GUIHelperInterface* m_guiHelper;
int m_sharedMemoryKey;
PhysicsServerInternalData() PhysicsServerInternalData()
@@ -52,7 +61,8 @@ struct PhysicsServerInternalData
m_isConnected(false), m_isConnected(false),
m_physicsDeltaTime(1./60.), m_physicsDeltaTime(1./60.),
m_dynamicsWorld(0), m_dynamicsWorld(0),
m_guiHelper(0) m_guiHelper(0),
m_sharedMemoryKey(SHARED_MEMORY_KEY)
{ {
} }
@@ -82,19 +92,114 @@ PhysicsServerSharedMemory::PhysicsServerSharedMemory()
m_data->m_sharedMemory = new PosixSharedMemory(); m_data->m_sharedMemory = new PosixSharedMemory();
#endif #endif
createEmptyDynamicsWorld();
} }
PhysicsServerSharedMemory::~PhysicsServerSharedMemory() PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
{ {
deleteDynamicsWorld();
delete m_data; delete m_data;
} }
void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
{
bool PhysicsServerSharedMemory::connectSharedMemory( class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper) m_data->m_sharedMemoryKey = key;
}
void PhysicsServerSharedMemory::createEmptyDynamicsWorld()
{
///collision configuration contains default setup for memory, collision setup
m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
//m_collisionConfiguration->setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration);
m_data->m_broadphase = new btDbvtBroadphase();
m_data->m_solver = new btMultiBodyConstraintSolver;
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
m_data->m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
}
void PhysicsServerSharedMemory::deleteDynamicsWorld()
{
for (int i=0;i<m_data->m_multiBodyJointFeedbacks.size();i++)
{
delete m_data->m_multiBodyJointFeedbacks[i];
}
m_data->m_multiBodyJointFeedbacks.clear();
for (int i=0;i<m_data->m_worldImporters.size();i++)
{
delete m_data->m_worldImporters[i];
}
m_data->m_worldImporters.clear();
for (int i=0;i<m_data->m_urdfLinkNameMapper.size();i++)
{
delete m_data->m_urdfLinkNameMapper[i];
}
m_data->m_urdfLinkNameMapper.clear();
m_data->m_multiBodyJointMotorMap.clear();
for (int i=0;i<m_data->m_strings.size();i++)
{
delete m_data->m_strings[i];
}
m_data->m_strings.clear();
if (m_data->m_dynamicsWorld)
{
int i;
for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
{
m_data->m_dynamicsWorld->removeConstraint(m_data->m_dynamicsWorld->getConstraint(i));
}
for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
{
btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i];
btRigidBody* body = btRigidBody::upcast(obj);
if (body && body->getMotionState())
{
delete body->getMotionState();
}
m_data->m_dynamicsWorld->removeCollisionObject(obj);
delete obj;
}
}
//delete collision shapes
for (int j = 0; j<m_data->m_collisionShapes.size(); j++)
{
btCollisionShape* shape = m_data->m_collisionShapes[j];
delete shape;
}
m_data->m_collisionShapes.clear();
delete m_data->m_dynamicsWorld;
delete m_data->m_solver;
delete m_data->m_broadphase;
delete m_data->m_dispatcher;
delete m_data->m_collisionConfiguration;
}
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
{ {
m_data->m_dynamicsWorld = dynamicsWorld;
m_data->m_guiHelper = guiHelper; m_data->m_guiHelper = guiHelper;
bool allowCreation = true; bool allowCreation = true;
@@ -107,7 +212,7 @@ bool PhysicsServerSharedMemory::connectSharedMemory( class btMultiBodyDynamicsWo
} }
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation); m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlock1) if (m_data->m_testBlock1)
{ {
int magicId =m_data->m_testBlock1->m_magicId; int magicId =m_data->m_testBlock1->m_magicId;
@@ -121,7 +226,7 @@ bool PhysicsServerSharedMemory::connectSharedMemory( class btMultiBodyDynamicsWo
} else } else
{ {
b3Error("Server cannot connect to existing shared memory, disconnecting shared memory.\n"); b3Error("Server cannot connect to existing shared memory, disconnecting shared memory.\n");
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0; m_data->m_testBlock1 = 0;
m_data->m_isConnected = false; m_data->m_isConnected = false;
} }
@@ -146,7 +251,7 @@ void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMe
b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId); b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId);
} }
btAssert(m_data->m_sharedMemory); btAssert(m_data->m_sharedMemory);
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
} }
if (m_data->m_sharedMemory) if (m_data->m_sharedMemory)
{ {
@@ -166,7 +271,8 @@ void PhysicsServerSharedMemory::releaseSharedMemory()
m_data->m_testBlock1->m_magicId = 0; m_data->m_testBlock1->m_magicId = 0;
b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId); b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId);
btAssert(m_data->m_sharedMemory); btAssert(m_data->m_sharedMemory);
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE); m_data->m_sharedMemory->releaseSharedMemory( m_data->m_sharedMemoryKey
, SHARED_MEMORY_SIZE);
} }
if (m_data->m_sharedMemory) if (m_data->m_sharedMemory)
{ {
@@ -684,11 +790,15 @@ void PhysicsServerSharedMemory::processClientCommands()
case CMD_SHUTDOWN: case CMD_RESET_SIMULATION:
{ {
btAssert(0); //clean up all data
//wantsShutdown = true;
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); m_data->m_guiHelper->getRenderInterface()->removeAllInstances();
deleteDynamicsWorld();
createEmptyDynamicsWorld();
SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(serverCmd); m_data->submitServerStatus(serverCmd);
break; break;
@@ -751,3 +861,28 @@ void PhysicsServerSharedMemory::processClientCommands()
} }
} }
} }
void PhysicsServerSharedMemory::renderScene()
{
if (m_data->m_guiHelper)
{
m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld);
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
}
}
void PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)
{
if (m_data->m_dynamicsWorld)
{
if (m_data->m_dynamicsWorld->getDebugDrawer())
{
m_data->m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
}
m_data->m_dynamicsWorld->debugDrawWorld();
}
}

View File

@@ -9,6 +9,8 @@ protected:
void createJointMotors(class btMultiBody* body); void createJointMotors(class btMultiBody* body);
virtual void createEmptyDynamicsWorld();
virtual void deleteDynamicsWorld();
void releaseSharedMemory(); void releaseSharedMemory();
@@ -19,8 +21,10 @@ public:
PhysicsServerSharedMemory(); PhysicsServerSharedMemory();
virtual ~PhysicsServerSharedMemory(); virtual ~PhysicsServerSharedMemory();
virtual void setSharedMemoryKey(int key);
//todo: implement option to allocated shared memory from client //todo: implement option to allocated shared memory from client
virtual bool connectSharedMemory(class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper); virtual bool connectSharedMemory( struct GUIHelperInterface* guiHelper);
virtual void disconnectSharedMemory (bool deInitializeSharedMemory); virtual void disconnectSharedMemory (bool deInitializeSharedMemory);
@@ -28,6 +32,12 @@ public:
bool supportsJointMotor(class btMultiBody* body, int linkIndex); bool supportsJointMotor(class btMultiBody* body, int linkIndex);
//for physicsDebugDraw and renderScene are mainly for debugging purposes
//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
//to a physics client, over shared memory
void physicsDebugDraw(int debugDrawFlags);
void renderScene();
}; };

View File

@@ -9,16 +9,7 @@
#include "PhysicsServer.h" #include "PhysicsServer.h"
#include "SharedMemoryCommon.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 class PhysicsServerExample : public SharedMemoryCommon
{ {
@@ -51,6 +42,19 @@ public:
virtual bool wantsTermination(); virtual bool wantsTermination();
virtual bool isConnected(); virtual bool isConnected();
virtual void renderScene();
virtual void exitPhysics(){}
virtual void physicsDebugDraw(int debugFlags);
virtual bool mouseMoveCallback(float x,float y){return false;};
virtual bool mouseButtonCallback(int button, int state, float x, float y){return false;}
virtual bool keyboardCallback(int key, int state){return false;}
virtual void setSharedMemoryKey(int key)
{
m_physicsServer.setSharedMemoryKey(key);
}
}; };
@@ -82,14 +86,18 @@ void PhysicsServerExample::initPhysics()
int upAxis = 2; int upAxis = 2;
m_guiHelper->setUpAxis(upAxis); m_guiHelper->setUpAxis(upAxis);
#if 0
createEmptyDynamicsWorld(); createEmptyDynamicsWorld();
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire //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); btVector3 grav(0,0,0);
grav[upAxis] = 0;//-9.8; grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav); this->m_dynamicsWorld->setGravity(grav);
m_isConnected = m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper); #endif
m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
} }
@@ -106,10 +114,30 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
m_physicsServer.processClientCommands(); m_physicsServer.processClientCommands();
} }
void PhysicsServerExample::renderScene()
{
///debug rendering
m_physicsServer.renderScene();
}
void PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
{
///debug rendering
m_physicsServer.physicsDebugDraw(debugDrawFlags);
}
extern int gSharedMemoryKey;
class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options) class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOptions& options)
{ {
return new PhysicsServerExample(options.m_guiHelper); PhysicsServerExample* example = new PhysicsServerExample(options.m_guiHelper);
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
}
return example;
} }

View File

@@ -18,8 +18,16 @@
struct PosixSharedMemoryInteralData struct PosixSharedMemoryInteralData
{ {
bool m_createdSharedMemory;
void* m_sharedMemoryPtr;
PosixSharedMemoryInteralData()
:m_createdSharedMemory(false),
m_sharedMemoryPtr(0)
{
}
}; };
PosixSharedMemory::PosixSharedMemory() PosixSharedMemory::PosixSharedMemory()
{ {
@@ -57,6 +65,8 @@ void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCr
b3Error("shmat returned -1"); b3Error("shmat returned -1");
} else } else
{ {
m_internalData->m_createdSharedMemory = allowCreation;
m_internalData->m_sharedMemoryPtr = result.ptr;
return result.ptr; return result.ptr;
} }
} }
@@ -76,11 +86,24 @@ void PosixSharedMemory::releaseSharedMemory(int key, int size)
b3Error("PosixSharedMemory::releaseSharedMemory: shmget error"); b3Error("PosixSharedMemory::releaseSharedMemory: shmget error");
} else } else
{ {
int result = shmctl(id,IPC_RMID,0); if (m_internalData->m_createdSharedMemory)
if (result == -1) {
{ int result = shmctl(id,IPC_RMID,0);
b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1"); if (result == -1)
} {
b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1");
} else
{
b3Printf("PosixSharedMemory::releaseSharedMemory removed shared memory");
}
m_internalData->m_createdSharedMemory = false;
m_internalData->m_sharedMemoryPtr = 0;
} else
{
shmdt(m_internalData->m_sharedMemoryPtr);
m_internalData->m_sharedMemoryPtr = 0;
b3Printf("PosixSharedMemory::releaseSharedMemory detached shared memory\n");
}
} }
#endif #endif
} }

View File

@@ -72,6 +72,20 @@ public:
virtual bool wantsTermination(); virtual bool wantsTermination();
virtual bool isConnected(); virtual bool isConnected();
virtual void renderScene(){}
virtual void exitPhysics(){}
virtual void physicsDebugDraw(int debugFlags){}
virtual bool mouseMoveCallback(float x,float y){return false;};
virtual bool mouseButtonCallback(int button, int state, float x, float y){return false;}
virtual bool keyboardCallback(int key, int state){return false;}
virtual void setSharedMemoryKey(int key)
{
m_physicsServer.setSharedMemoryKey(key);
m_physicsClient.setSharedMemoryKey(key);
}
}; };
bool RobotControlExample::isConnected() bool RobotControlExample::isConnected()
@@ -253,14 +267,14 @@ void RobotControlExample::initPhysics()
int upAxis = 2; int upAxis = 2;
m_guiHelper->setUpAxis(upAxis); m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld(); /* createEmptyDynamicsWorld();
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire //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); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 grav(0,0,0); btVector3 grav(0,0,0);
grav[upAxis] = 0;//-9.8; grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav); this->m_dynamicsWorld->setGravity(grav);
*/
m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper); m_physicsServer.connectSharedMemory( m_guiHelper);
if (m_guiHelper && m_guiHelper->getParameterInterface()) if (m_guiHelper && m_guiHelper->getParameterInterface())
{ {
@@ -364,10 +378,16 @@ void RobotControlExample::stepSimulation(float deltaTime)
} }
} }
extern int gSharedMemoryKey;
class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options) class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options)
{ {
return new RobotControlExample(options.m_guiHelper); RobotControlExample* example = new RobotControlExample(options.m_guiHelper);
if (gSharedMemoryKey>=0)
{
example->setSharedMemoryKey(gSharedMemoryKey);
}
return example;
} }

View File

@@ -37,7 +37,7 @@ enum EnumSharedMemoryClientCommand
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE? CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
CMD_REQUEST_ACTUAL_STATE, CMD_REQUEST_ACTUAL_STATE,
CMD_STEP_FORWARD_SIMULATION, CMD_STEP_FORWARD_SIMULATION,
CMD_SHUTDOWN, CMD_RESET_SIMULATION,
CMD_MAX_CLIENT_COMMANDS CMD_MAX_CLIENT_COMMANDS
}; };

View File

@@ -3,14 +3,19 @@
#include "../CommonInterfaces/CommonMultiBodyBase.h" #include "../CommonInterfaces/CommonMultiBodyBase.h"
class SharedMemoryCommon : public CommonMultiBodyBase class SharedMemoryCommon : public CommonExampleInterface
{ {
protected:
struct GUIHelperInterface* m_guiHelper;
public: public:
SharedMemoryCommon(GUIHelperInterface* helper) SharedMemoryCommon(GUIHelperInterface* helper)
:CommonMultiBodyBase(helper) :m_guiHelper(helper)
{ {
} }
virtual void setSharedMemoryKey(int key)=0;
virtual bool wantsTermination()=0; virtual bool wantsTermination()=0;
virtual bool isConnected()=0; virtual bool isConnected()=0;
}; };

View File

@@ -25,6 +25,7 @@ subject to the following restrictions:
#include <stdlib.h> #include <stdlib.h>
int gSharedMemoryKey = -1;
static SharedMemoryCommon* example = NULL; static SharedMemoryCommon* example = NULL;
static bool interrupted = false; static bool interrupted = false;
@@ -70,6 +71,8 @@ int main(int argc, char* argv[])
CommonExampleOptions options(&noGfx); CommonExampleOptions options(&noGfx);
args.GetCmdLineArgument("shared_memory_key", gSharedMemoryKey);
if (args.CheckCmdLineFlag("client")) if (args.CheckCmdLineFlag("client"))
{ {
example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options); example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options);
@@ -78,6 +81,7 @@ int main(int argc, char* argv[])
example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options); example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
} }
example->initPhysics(); example->initPhysics();
while (example->isConnected() && !(example->wantsTermination() || interrupted)) while (example->isConnected() && !(example->wantsTermination() || interrupted))
{ {