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)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
@@ -347,6 +350,20 @@ int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemory
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)

16
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -15,18 +15,26 @@ B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle);
extern "C" {
#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);
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient);
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
///check if a command can be send
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);
///non-blocking check status
b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient);
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetNumJoints(b3PhysicsClientHandle physClient);
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>
#define MAX_TIMEOUT 1024*1024*1024
int main(int argc, char* argv[])
{
int i, dofCount , posVarCount, dofIndex, ret ,numJoints, allowSharedMemoryInitialization=0;
int timeout = MAX_TIMEOUT;
int sensorJointIndexLeft=-1;
int sensorJointIndexRight=-1;
int statusType = -1;
const char* urdfFileName = "r2d2.urdf";
double gravx=0, gravy=0, gravz=-9.8;
double timeStep = 1./60.;
@@ -26,8 +25,6 @@ int main(int argc, char* argv[])
b3PhysicsClientHandle sm;
b3Printf("timeout = %d\n",timeout);
printf("hello world\n");
sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
@@ -37,10 +34,7 @@ int main(int argc, char* argv[])
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz);
ret = b3PhysicsParamSetTimeStep(command, timeStep);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3Printf("timeout = %d\n",timeout);
b3SubmitClientCommandAndWaitStatus(sm, command);
}
{
@@ -50,11 +44,7 @@ int main(int argc, char* argv[])
startPosY =3;
startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3Printf("timeout = %d\n",timeout);
b3SubmitClientCommandAndWaitStatus(sm, command);
}
numJoints = b3GetNumJoints(sm);
@@ -84,8 +74,8 @@ int main(int argc, char* argv[])
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0)
{
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
@@ -99,64 +89,61 @@ int main(int argc, char* argv[])
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
}
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
{
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm);
ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1);
ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1);
ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
{
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
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);
for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
{
b3JointControlSetDesiredVelocity(command,dofIndex,1);
b3JointControlSetMaximumForce(command,dofIndex,100);
}
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
///perform some simulation steps for testing
for ( i=0;i<100;i++)
{
b3SharedMemoryCommandHandle command = b3InitStepSimulationCommand(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
}
{
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm));
}
#if 0
if (sensorJointIndexLeft>=0)
{
@@ -175,10 +162,7 @@ int main(int argc, char* argv[])
}
#endif
{
b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
}
}