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

another step closer to useable shared memory C API

Browse Source 
(force/torque sensor needs new API)
in a nutshell, users of shared memory physics API should not
directly poke into shared memory, not fill 'SharedMemorCommand'
nor read SharedMemoryStatus directly. The C-API declares 'handles' for those,
to avoid it from happening.
This commit is contained in:
=
2015-09-16 23:09:10 -07:00
parent 4041748f55
commit 003a42478b
16 changed files with 671 additions and 489 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
test/SharedMemory/test.c
View File

@@ -1,6 +1,5 @@
#include "SharedMemoryBlock.h"
//#include "SharedMemoryCommands.h"
#include "PhysicsClientC_API.h"
#include "SharedMemoryCommands.h"
#include "Bullet3Common/b3Logging.h"
#include <string.h>
@@ -24,39 +23,39 @@ int main(int argc, char* argv[])
double startPosX, startPosY,startPosZ;
int imuLinkIndex = -1;
SharedMemoryCommand_t command;
SharedMemoryStatus_t status;
b3PhysicsClientHandle sm;
b3Printf("timeout = %d\n",timeout);
printf("hello world\n");
sm = b3ConnectSharedMemory( allowSharedMemoryInitialization);
sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
if (b3CanSubmitCommand(sm))
{
ret = b3InitPhysicsParamCommand(&command);
ret = b3PhysicsParamSetGravity(&command, gravx,gravy, gravz);
ret = b3PhysicsParamSetTimeStep(&command, timeStep);
ret = b3SubmitClientCommand(sm, &command);
{
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, &status)==0) {}
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
b3Printf("timeout = %d\n",timeout);
}
ret = b3LoadUrdfCommandInit(&command, urdfFileName);
//setting the initial position, orientation and other arguments are optional
startPosX =2;
startPosY =3;
startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(&command, startPosX,startPosY,startPosZ);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
b3Printf("timeout = %d\n",timeout);
{
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
//setting the initial position, orientation and other arguments are optional
startPosX =2;
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);
}
numJoints = b3GetNumJoints(sm);
for (i=0;i<numJoints;i++)
@@ -85,70 +84,80 @@ int main(int argc, char* argv[])
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
ret = b3CreateSensorCommandInit(&command);
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
if (imuLinkIndex>=0)
{
ret = b3CreateSensorEnableIMUForLink(&command, imuLinkIndex, 1);
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
}
if (sensorJointIndexLeft>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1);
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
}
if(sensorJointIndexRight>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexRight, 1);
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
}
ret = b3SubmitClientCommand(sm, &command);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
}
ret = b3CreateBoxShapeCommandInit(&command);
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, &status)==0) {}
b3RequestActualStateCommandInit(&command);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
{
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) {}
}
{
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
}
#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
b3JointControlCommandInit(&command, CONTROL_MODE_VELOCITY);
for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
{
b3JointControlSetDesiredVelocity(&command,dofIndex,1);
b3JointControlSetMaximumForce(&command,dofIndex,100);
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) {}
}
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
///perform some simulation steps for testing
for ( i=0;i<100;i++)
{
ret = b3InitStepSimulationCommand(&command);
ret = b3SubmitClientCommand(sm, &command);
b3SharedMemoryCommandHandle command = b3InitStepSimulationCommand(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
}
b3RequestActualStateCommandInit(&command);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
{
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
}
#if 0
if (sensorJointIndexLeft>=0)
{
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
@@ -164,12 +173,13 @@ int main(int argc, char* argv[])
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]);
}
ret = b3InitResetSimulationCommand(&command);
ret = b3SubmitClientCommand(sm, &command);
#endif
{
b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(sm);
ret = b3SubmitClientCommand(sm, command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
while ((timeout-- > 0) && b3ProcessServerStatus(sm)==0) {}
}
}