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

more work on physics client c-api over shared memory

Browse Source
This commit is contained in:
=
2015-09-17 09:37:44 -07:00
parent 003a42478b
commit 23bdd0ed36
3 changed files with 61 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -322,6 +322,9 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien
} }
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle) int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -347,6 +350,20 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemory
return (int)cl->submitClientCommand(*command); return (int)cl->submitClientCommand(*command);
} }
b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
{
int timeout = 1024*1024*1024;
b3SharedMemoryStatusHandle statusHandle=0;
b3SubmitClientCommand(physClient,commandHandle);
while ((statusHandle==0) && (timeout-- > 0))
{
statusHandle =b3ProcessServerStatus(physClient);
}
return (b3SharedMemoryStatusHandle) statusHandle;
}
int b3GetNumJoints(b3PhysicsClientHandle physClient) int b3GetNumJoints(b3PhysicsClientHandle physClient)

16
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -15,18 +15,26 @@ B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle);
extern "C" { extern "C" {
#endif #endif
///make sure to start the server first! ///make sure to start the server first, before connecting client to physics server
b3PhysicsClientHandle b3ConnectSharedMemory(int key); b3PhysicsClientHandle b3ConnectSharedMemory(int key);
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient); void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient); ///check if a command can be send
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3CanSubmitCommand(b3PhysicsClientHandle physClient); int b3CanSubmitCommand(b3PhysicsClientHandle physClient);
//blocking submit command and wait for status
b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
///non-blocking submit command
int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle); int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
///non-blocking check status
b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient);
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetNumJoints(b3PhysicsClientHandle physClient); int b3GetNumJoints(b3PhysicsClientHandle physClient);
void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info); void b3GetJointInfo(b3PhysicsClientHandle physClient, int linkIndex, struct b3JointInfo* info);

80
test/SharedMemory/test.c
View File

@@ -9,14 +9,13 @@ struct test
}; };
#include <stdio.h> #include <stdio.h>
#define MAX_TIMEOUT 1024*1024*1024
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
int i, dofCount , posVarCount, dofIndex, ret ,numJoints, allowSharedMemoryInitialization=0; int i, dofCount , posVarCount, dofIndex, ret ,numJoints, allowSharedMemoryInitialization=0;
int timeout = MAX_TIMEOUT;
int sensorJointIndexLeft=-1; int sensorJointIndexLeft=-1;
int sensorJointIndexRight=-1; int sensorJointIndexRight=-1;
int statusType = -1;
const char* urdfFileName = "r2d2.urdf"; const char* urdfFileName = "r2d2.urdf";
double gravx=0, gravy=0, gravz=-9.8; double gravx=0, gravy=0, gravz=-9.8;
double timeStep = 1./60.; double timeStep = 1./60.;
@@ -26,8 +25,6 @@ int main(int argc, char* argv[])
b3PhysicsClientHandle sm; b3PhysicsClientHandle sm;
b3Printf("timeout = %d\n",timeout);
printf("hello world\n"); printf("hello world\n");
sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
@@ -37,10 +34,7 @@ int main(int argc, char* argv[])
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz); ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz);
ret = b3PhysicsParamSetTimeStep(command, timeStep); ret = b3PhysicsParamSetTimeStep(command, timeStep);
ret = b3SubmitClientCommand(sm, command); b3SubmitClientCommandAndWaitStatus(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3Printf("timeout = %d\n",timeout);
} }
{ {
@@ -50,11 +44,7 @@ int main(int argc, char* argv[])
startPosY =3; startPosY =3;
startPosZ = 1; startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
b3SubmitClientCommandAndWaitStatus(sm, command);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3Printf("timeout = %d\n",timeout);
} }
numJoints = b3GetNumJoints(sm); numJoints = b3GetNumJoints(sm);
@@ -84,8 +74,8 @@ int main(int argc, char* argv[])
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0)) if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{ {
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm); b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0) if (imuLinkIndex>=0)
{ {
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1); ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
@@ -99,64 +89,61 @@ int main(int argc, char* argv[])
{ {
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1); ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
} }
ret = b3SubmitClientCommand(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
} }
{ {
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm); b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm);
ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1); ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1);
ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1); ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1);
ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1); ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1);
ret = b3SubmitClientCommand(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
} }
{ {
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm); b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm);
ret = b3SubmitClientCommand(sm, command); b3SharedMemoryStatusHandle statusHandle;
timeout = MAX_TIMEOUT; statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {} int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
#if 0
posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("posVarCount = %d\n",posVarCount);
printf("dofCount = %d\n",dofCount);
#endif
}
} }
#if 0
posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("posVarCount = %d\n",posVarCount);
printf("dofCount = %d\n",dofCount);
#endif
{ {
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY); b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
for ( dofIndex=0;dofIndex<dofCount;dofIndex++) for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
{ {
b3JointControlSetDesiredVelocity(command,dofIndex,1); b3JointControlSetDesiredVelocity(command,dofIndex,1);
b3JointControlSetMaximumForce(command,dofIndex,100); b3JointControlSetMaximumForce(command,dofIndex,100);
} }
ret = b3SubmitClientCommand(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
} }
///perform some simulation steps for testing ///perform some simulation steps for testing
for ( i=0;i<100;i++) for ( i=0;i<100;i++)
{ {
b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
b3SharedMemoryCommandHandle command = b3InitStepSimulationCommand(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
} }
{ {
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm); b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm));
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
} }
#if 0 #if 0
if (sensorJointIndexLeft>=0) if (sensorJointIndexLeft>=0)
{ {
@@ -175,10 +162,7 @@ int main(int argc, char* argv[])
} }
#endif #endif
{ {
b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(sm); b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
} }
} }