Merge pull request #451 from erwincoumans/master

shared memory fixes
This commit is contained in:
erwincoumans
2015-08-06 12:35:40 -07:00
19 changed files with 231 additions and 109 deletions

Binary file not shown.

View File

@@ -266,9 +266,8 @@ void openFileDemo(const char* filename)
s_parameterInterface->removeAllParameters();
CommonExampleOptions options(s_guiHelper,0);
CommonExampleOptions options(s_guiHelper,1);
options.m_fileName = filename;
char fullPath[1024];
sprintf(fullPath, "%s", filename);
b3FileUtils::toLower(fullPath);

View File

@@ -13,6 +13,9 @@
#include "ExampleEntries.h"
#include "Bullet3Common/b3Logging.h"
int main(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);

View File

@@ -794,8 +794,8 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
vis.m_localMaterial = **mat;
} else
{
logger->reportError("Cannot find material with name:");
logger->reportError(vis.m_materialName.c_str());
//logger->reportError("Cannot find material with name:");
//logger->reportError(vis.m_materialName.c_str());
}
}
}

View File

@@ -493,7 +493,10 @@ int stbi_write_png(char const *filename, int x, int y, int comp, const void *dat
unsigned char *png = stbi_write_png_to_mem((unsigned char *) data, stride_bytes, x, y, comp, &len);
if (!png) return 0;
f = fopen(filename, "wb");
if (!f) { free(png); return 0; }
if (!f) {
free(png);
return 0;
}
fwrite(png, 1, len, f);
fclose(f);
free(png);

View File

@@ -229,6 +229,9 @@ void RollingFrictionDemo::initPhysics()
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
if (0)
{
btSerializer* s = new btDefaultSerializer;
m_dynamicsWorld->serialize(s);
b3ResourcePath p;
@@ -239,6 +242,7 @@ void RollingFrictionDemo::initPhysics()
fwrite(s->getBufferPointer(),s->getCurrentBufferSize(),1,f);
fclose(f);
}
}
}

View File

@@ -77,38 +77,43 @@ PhysicsClientSharedMemory::PhysicsClientSharedMemory()
PhysicsClientSharedMemory::~PhysicsClientSharedMemory()
{
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
if (m_data->m_isConnected)
{
disconnectSharedMemory();
}
delete m_data->m_sharedMemory;
delete m_data;
}
void PhysicsClientSharedMemory::disconnectSharedMemory ()
{
if (m_data->m_isConnected)
{
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_data->m_isConnected = false;
}
}
bool PhysicsClientSharedMemory::isConnected() const
{
return m_data->m_isConnected ;
}
bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
bool PhysicsClientSharedMemory::connect()
{
bool allowCreation = true;
///server always has to create and initialize shared memory
bool allowCreation = false;
m_data->m_testBlock1 = (SharedMemoryBlock*)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)
{
InitSharedMemoryBlock(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
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;

View File

@@ -15,7 +15,9 @@ public:
virtual ~PhysicsClientSharedMemory();
//return true if connection succesfull, can also check 'isConnected'
virtual bool connect(bool allowSharedMemoryInitialization = true);
virtual bool connect();
virtual void disconnectSharedMemory ();
virtual bool isConnected() const;

View File

@@ -191,10 +191,11 @@ int b3CreateSensorEnable6DofJointForceTorqueSensor(struct SharedMemoryCommand* c
}
b3PhysicsClientHandle b3ConnectSharedMemory( int allowSharedMemoryInitialization)
b3PhysicsClientHandle b3ConnectSharedMemory()
{
PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory();
cl->connect(allowSharedMemoryInitialization);
///client should never create shared memory, only the server does
cl->connect();
return (b3PhysicsClientHandle ) cl;
}

View File

@@ -12,7 +12,8 @@ B3_DECLARE_HANDLE(b3PhysicsRobotHandle);
extern "C" {
#endif
b3PhysicsClientHandle b3ConnectSharedMemory(int allowSharedMemoryInitialization);
///make sure to start the server first!
b3PhysicsClientHandle b3ConnectSharedMemory();
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);

View File

@@ -9,6 +9,15 @@
#include "PhysicsClient.h"
#include "SharedMemoryCommands.h"
struct MyMotorInfo2
{
btScalar m_velTarget;
btScalar m_maxForce;
int m_uIndex;
};
#define MAX_NUM_MOTORS 128
class PhysicsClientExample : public SharedMemoryCommon
{
protected:
@@ -22,6 +31,11 @@ protected:
public:
//@todo, add accessor methods
MyMotorInfo2 m_motorTargetVelocities[MAX_NUM_MOTORS];
int m_numMotors;
PhysicsClientExample(GUIHelperInterface* helper);
virtual ~PhysicsClientExample();
@@ -42,11 +56,20 @@ public:
{
return m_wantsTermination;
}
virtual bool isConnected()
{
return m_physicsClient.isConnected();
}
void enqueueCommand(const SharedMemoryCommand& orgCommand);
};
void MyCallback(int buttonId, bool buttonState, void* userPtr)
{
PhysicsClientExample* cl = (PhysicsClientExample*) userPtr;
@@ -58,8 +81,8 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
case CMD_LOAD_URDF:
{
command.m_type =CMD_LOAD_URDF;
sprintf(command.m_urdfArguments.m_urdfFileName,"hinge.urdf");//r2d2.urdf");
command.m_urdfArguments.m_initialPosition[0] = 0.0;
sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf");
command.m_urdfArguments.m_initialPosition[0] = 0.0;
command.m_urdfArguments.m_initialPosition[1] = 0.0;
command.m_urdfArguments.m_initialPosition[2] = 0.0;
command.m_urdfArguments.m_initialOrientation[0] = 0.0;
@@ -92,36 +115,29 @@ void MyCallback(int buttonId, bool buttonState, void* userPtr)
case CMD_SEND_DESIRED_STATE:
{
command.m_type =CMD_SEND_DESIRED_STATE;
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
command.m_type =CMD_SEND_DESIRED_STATE;
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
//todo: expose a drop box in the GUI for this
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 1;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
}
break;
}
case CONTROL_MODE_TORQUE:
{
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100;
}
break;
}
default:
{
b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
btAssert(0);
}
}
command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
}
for (int i=0;i<cl->m_numMotors;i++)
{
btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget;
int uIndex = cl->m_motorTargetVelocities[i].m_uIndex;
if (targetVel>1)
{
printf("testme");
}
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
}
cl->enqueueCommand(command);
break;
}
@@ -200,7 +216,7 @@ void PhysicsClientExample::initPhysics()
if (!m_physicsClient.connect())
{
b3Warning("Cannot eonnect to physics client");
b3Warning("Cannot connect to physics client");
}
}
@@ -222,6 +238,36 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumJoints();i++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
}
}
}
}
}
if (m_physicsClient.canSubmitCommand())

View File

@@ -92,7 +92,7 @@ PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper)
bool PhysicsServerSharedMemory::connectSharedMemory( class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper)
{
m_data->m_dynamicsWorld = dynamicsWorld;
m_data->m_guiHelper = guiHelper;
@@ -100,34 +100,37 @@ bool PhysicsServerSharedMemory::connectSharedMemory(bool allowSharedMemoryInitia
bool allowCreation = true;
bool allowConnectToExistingSharedMemory = false;
if (m_data->m_isConnected)
{
b3Warning("connectSharedMemory, while already connected");
return m_data->m_isConnected;
}
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlock1)
{
if (!allowConnectToExistingSharedMemory || (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER))
int magicId =m_data->m_testBlock1->m_magicId;
b3Printf("magicId = %d\n", magicId);
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
if (allowSharedMemoryInitialization)
{
InitSharedMemoryBlock(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;
}
InitSharedMemoryBlock(m_data->m_testBlock1);
b3Printf("Created and initialized shared memory block\n");
m_data->m_isConnected = true;
} else
{
b3Printf("Connected to existing shared memory, status OK.\n");
m_data->m_isConnected = true;
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_testBlock1 = 0;
m_data->m_isConnected = false;
}
} else
{
b3Error("Cannot connect to shared memory");
return false;
m_data->m_isConnected = false;
}
return true;
return m_data->m_isConnected;
}
@@ -693,7 +696,7 @@ void PhysicsServerSharedMemory::processClientCommands()
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
btVector3 halfExtents(1,1,1);
if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_HALF_EXTENTS)
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_HALF_EXTENTS)
{
halfExtents = btVector3(
clientCmd.m_createBoxShapeArguments.m_halfExtentsX,
@@ -702,7 +705,7 @@ void PhysicsServerSharedMemory::processClientCommands()
}
btTransform startTrans;
startTrans.setIdentity();
if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_POSITION)
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_POSITION)
{
startTrans.setOrigin(btVector3(
clientCmd.m_createBoxShapeArguments.m_initialPosition[0],
@@ -710,8 +713,10 @@ void PhysicsServerSharedMemory::processClientCommands()
clientCmd.m_createBoxShapeArguments.m_initialPosition[2]));
}
if (clientCmd.m_updateFlags | BOX_SHAPE_HAS_INITIAL_ORIENTATION)
if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION)
{
b3Printf("test\n");
startTrans.setRotation(btQuaternion(
clientCmd.m_createBoxShapeArguments.m_initialOrientation[0],
clientCmd.m_createBoxShapeArguments.m_initialOrientation[1],

View File

@@ -20,7 +20,7 @@ public:
virtual ~PhysicsServerSharedMemory();
//todo: implement option to allocated shared memory from client
virtual bool connectSharedMemory(bool allowSharedMemoryInitialization, class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper);
virtual bool connectSharedMemory(class btMultiBodyDynamicsWorld* dynamicsWorld, struct GUIHelperInterface* guiHelper);
virtual void disconnectSharedMemory (bool deInitializeSharedMemory);

View File

@@ -24,9 +24,9 @@ class PhysicsServerExample : public SharedMemoryCommon
{
PhysicsServerSharedMemory m_physicsServer;
bool m_wantsShutdown;
bool m_wantsShutdown;
bool m_isConnected;
public:
@@ -50,11 +50,14 @@ public:
}
virtual bool wantsTermination();
virtual bool isConnected();
};
PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_wantsShutdown(false)
m_wantsShutdown(false),
m_isConnected(false)
{
b3Printf("Started PhysicsServer\n");
}
@@ -65,6 +68,12 @@ PhysicsServerExample::~PhysicsServerExample()
{
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
m_isConnected = false;
}
bool PhysicsServerExample::isConnected()
{
return m_isConnected;
}
void PhysicsServerExample::initPhysics()
@@ -80,8 +89,7 @@ void PhysicsServerExample::initPhysics()
grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav);
bool allowSharedMemoryInitialization = true;
m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper);
m_isConnected = m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper);
}

View File

@@ -40,10 +40,10 @@ struct btPointerCaster
};
};
void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool /*allowCreation*/)
void* PosixSharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
{
#ifdef TEST_SHARED_MEMORY
int flags = IPC_CREAT | 0666;
int flags = (allowCreation ? IPC_CREAT : 0) | 0666;
int id = shmget((key_t) key, (size_t) size,flags);
if (id < 0)
{

View File

@@ -71,8 +71,13 @@ public:
}
virtual bool wantsTermination();
virtual bool isConnected();
};
bool RobotControlExample::isConnected()
{
return m_physicsClient.isConnected();
}
void MyCallback2(int buttonId, bool buttonState, void* userPtr)
{
@@ -131,6 +136,12 @@ void MyCallback2(int buttonId, bool buttonState, void* userPtr)
case 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);
break;
}
@@ -232,6 +243,7 @@ m_numMotors(0)
RobotControlExample::~RobotControlExample()
{
bool deInitializeSharedMemory = true;
m_physicsClient.disconnectSharedMemory();
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
@@ -248,8 +260,7 @@ void RobotControlExample::initPhysics()
grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav);
bool allowSharedMemoryInitialization = true;
m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper);
m_physicsServer.connectSharedMemory( m_dynamicsWorld,m_guiHelper);
if (m_guiHelper && m_guiHelper->getParameterInterface())
{

View File

@@ -12,6 +12,7 @@ public:
}
virtual bool wantsTermination()=0;
virtual bool isConnected()=0;
};
#endif//

View File

@@ -23,26 +23,63 @@ subject to the following restrictions:
#include "SharedMemoryCommon.h"
#include <stdlib.h>
static SharedMemoryCommon* example = NULL;
static bool interrupted = false;
#ifndef _WIN32
#include <signal.h>
#include <err.h>
#include <unistd.h>
static void cleanup(int signo)
{
if (interrupted) { // this is the second time, we're hanging somewhere
// if (example) {
// example->abort();
// }
b3Printf("Aborting and deleting SharedMemoryCommon object");
sleep(1);
delete example;
errx(EXIT_FAILURE, "aborted example on signal %d", signo);
}
interrupted = true;
warnx("caught signal %d", signo);
}
#endif//_WIN32
int main(int argc, char* argv[])
{
#ifndef _WIN32
struct sigaction action;
memset(&action, 0x0, sizeof(action));
action.sa_handler = cleanup;
static const int signos[] = { SIGHUP, SIGINT, SIGQUIT, SIGABRT, SIGSEGV, SIGPIPE, SIGTERM };
for (int ii(0); ii < sizeof(signos) / sizeof(*signos); ++ii) {
if (0 != sigaction(signos[ii], &action, NULL)) {
err(EXIT_FAILURE, "signal %d", signos[ii]);
}
}
#endif
b3CommandLineArgs args(argc, argv);
DummyGUIHelper noGfx;
CommonExampleOptions options(&noGfx);
SharedMemoryCommon* example = 0;
if (args.CheckCmdLineFlag("client"))
{
example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options);
}else
{
example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
}
{
example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options);
}else
{
example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
}
example->initPhysics();
while (!example->wantsTermination())
while (example->isConnected() && !(example->wantsTermination() || interrupted))
{
example->stepSimulation(1.f/60.f);
}

View File

@@ -61,6 +61,9 @@ struct LWRightBody
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
//btQuaternion q_w = [ sin(|w|*dt/2) * w/|w| , cos(|w|*dt/2)]
//btQuaternion q_new = q_w * q_old;
btVector3 axis;
btScalar fAngle = m_angularVelocity.length();
//limit the angular motion
@@ -88,6 +91,7 @@ struct LWRightBody
predictedOrn.normalize();
m_worldPose.m_worldOrientation = predictedOrn;
}
}
@@ -99,13 +103,9 @@ struct LWRightBody
///quick demo showing the right-handed coordinate system and positive rotations around each axis
class Tutorial : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
float m_x;
float m_y;
float m_z;
int m_tutorialIndex;
LWRightBody* m_body;
@@ -114,14 +114,14 @@ public:
Tutorial(CommonGraphicsApp* app, int tutorialIndex)
:m_app(app),
m_x(0),
m_y(0),
m_z(0),
m_tutorialIndex(tutorialIndex)
{
m_app->setUpAxis(2);
switch (m_tutorialIndex)
{
}
{
int boxId = m_app->registerCubeShape(100,100,1);
@@ -186,10 +186,6 @@ public:
}
virtual void stepSimulation(float deltaTime)
{
m_x+=0.01f;
m_y+=0.01f;
m_z+=0.01f;
//m_body->m_worldPose.m_worldPosition+= btVector3(0,0.01,0);
//m_body->m_linearVelocity=btVector3(0,0.1,0);
m_body->m_angularVelocity =btVector3(0,0.1,0);
m_body->integrateTransform(deltaTime);