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

small improvement to the shared memory physics server/client

Browse Source
This commit is contained in:
=
2015-05-28 17:49:55 -07:00
parent 82576d0bee
commit 7abefca6f1
4 changed files with 96 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
examples/SharedMemory/PhysicsClient.cpp
View File

@@ -49,7 +49,13 @@ void PhysicsClient::initPhysics()
m_testBlock1 = (SharedMemoryExampleData*)m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
if (m_testBlock1)
{
btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
// btAssert(m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
b3Error("Error: please start server before client");
m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_testBlock1 = 0;
}
}
}
@@ -66,7 +72,8 @@ void PhysicsClient::stepSimulation(float deltaTime)
once=false;
b3Printf("Client created CMD_LOAD_URDF");
m_testBlock1->m_clientCommands[0] =CMD_LOAD_URDF;
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_numClientCommands++;
}
}

57
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -3,6 +3,10 @@
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "PosixSharedMemory.h"
#include "../Importers/ImportURDFDemo/MyURDFImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
@@ -21,6 +25,9 @@ public:
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 = 1;
@@ -53,6 +60,8 @@ PhysicsServer::~PhysicsServer()
void PhysicsServer::initPhysics()
{
createEmptyDynamicsWorld();
m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
// btAssert(m_testBlock1);
@@ -77,6 +86,31 @@ void PhysicsServer::initPhysics()
}
}
bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase)
{
MyURDFImporter u2b(m_guiHelper);
bool loadOk = u2b.loadURDF(fileName);
if (loadOk)
{
b3Printf("loaded %s OK!", fileName);
btTransform tr;
tr.setIdentity();
tr.setOrigin(pos);
tr.setRotation(orn);
int rootLinkIndex = u2b.getRootLinkIndex();
// printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
bool m_useMultiBody = true;
ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
btMultiBody* mb = creation.getBulletMultiBody();
return false;
}
}
void PhysicsServer::stepSimulation(float deltaTime)
{
@@ -89,20 +123,31 @@ void PhysicsServer::stepSimulation(float deltaTime)
//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);
const SharedMemoryCommand& clientCmd =m_testBlock1->m_clientCommands[0];
//consume the command
switch (m_testBlock1->m_clientCommands[0])
switch (clientCmd.m_type)
{
case CMD_LOAD_URDF:
{
b3Printf("Processed CMD_LOAD_URDF");
b3Printf("Processed CMD_LOAD_URDF:%s",clientCmd.m_urdfArguments.m_urdfFileName);
//load the actual URDF and send a report: completed or failed
m_testBlock1->m_serverCommands[0] =CMD_URDF_LOADING_COMPLETED;
m_testBlock1->m_numServerCommands++;
//CMD_URDF_LOADING_COMPLETED,
//CMD_URDF_LOADING_FAILED,
bool completedOk = loadUrdf(clientCmd.m_urdfArguments.m_urdfFileName,
btVector3(0,0,0), btQuaternion(0,0,0,1),true,true );
SharedMemoryCommand& serverCmd =m_testBlock1->m_serverCommands[0];
if (completedOk)
{
serverCmd.m_type =CMD_URDF_LOADING_COMPLETED;
} else
{
serverCmd.m_type =CMD_URDF_LOADING_FAILED;
}
m_testBlock1->m_numServerCommands++;
}
default:
{

8
examples/SharedMemory/PosixSharedMemory.cpp
View File

@@ -1,8 +1,10 @@
#include "PosixSharedMemory.h"
#include "Bullet3Common/b3Logging.h"
#include "LinearMath/btScalar.h" //for btAssert
#ifdef __APPLE__
//#define TEST_SHARED_MEMORY
#define TEST_SHARED_MEMORY
#endif
#include <stddef.h>
@@ -71,13 +73,13 @@ void PosixSharedMemory::releaseSharedMemory(int key, int size)
int id = shmget((key_t) key, (size_t) size,flags);
if (id < 0)
{
b3Error("shmget error");
b3Error("PosixSharedMemory::releaseSharedMemory: shmget error");
} else
{
int result = shmctl(id,IPC_RMID,0);
if (result == -1)
{
b3Error("shmat returned -1");
b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1");
}
}
#endif

40
examples/SharedMemory/SharedMemoryInterface.h
View File

@@ -1,7 +1,7 @@
#ifndef SHARED_MEMORY_INTERFACE_H
#define SHARED_MEMORY_INTERFACE_H
#define SHARED_MEMORY_KEY 12345
#define SHARED_MEMORY_KEY 12347
#define SHARED_MEMORY_MAGIC_NUMBER 64738
#define SHARED_MEMORY_MAX_COMMANDS 64
@@ -26,17 +26,33 @@ enum SharedMemoryClientCommand{
#define MAX_NUM_SENSORS 1024
#define MAX_URDF_FILENAME_LENGTH 1024
struct CommandArguments
struct UrdfCommandArgument
{
char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
};
struct StepSimulationCommandArgument
{
double m_deltaTimeInSeconds;
char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
};
struct SharedMemoryCommand
{
int m_type;
union
{
UrdfCommandArgument m_urdfArguments;
StepSimulationCommandArgument m_stepSimulationArguments;
};
};
struct SharedMemoryExampleData
{
int m_magicId;
int m_clientCommands[SHARED_MEMORY_MAX_COMMANDS];
int m_serverCommands[SHARED_MEMORY_MAX_COMMANDS];
SharedMemoryCommand m_clientCommands[SHARED_MEMORY_MAX_COMMANDS];
SharedMemoryCommand m_serverCommands[SHARED_MEMORY_MAX_COMMANDS];
int m_numClientCommands;
int m_numProcessedClientCommands;
@@ -45,12 +61,16 @@ struct SharedMemoryExampleData
int m_numProcessedServerCommands;
double m_stateQ[MAX_DEGREE_OF_FREEDOM];
double m_stateQdot[MAX_DEGREE_OF_FREEDOM];
double m_stateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
//desired state is only written by the client, read-only access by server is expected
double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_desiredStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
//actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
double m_actualStateSensors[MAX_NUM_SENSORS];//these are force sensors and IMU information
CommandArguments m_clientCommandArguments[SHARED_MEMORY_MAX_COMMANDS];
CommandArguments m_serverCommandArguments[SHARED_MEMORY_MAX_COMMANDS];
};