void testSharedMemory(b3PhysicsClientHandle sm)
{
int i, dofCount , posVarCount, ret ,numJoints ;
int sensorJointIndexLeft=-1;
int sensorJointIndexRight=-1;
const char* urdfFileName = "r2d2.urdf";
const char* sdfFileName = "kuka_iiwa/model.sdf";
double gravx=0, gravy=0, gravz=-9.8;
double timeStep = 1./60.;
double startPosX, startPosY,startPosZ;
int imuLinkIndex = -1;
int bodyIndex = -1;
if (b3CanSubmitCommand(sm))
{
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz);
ret = b3PhysicsParamSetTimeStep(command, timeStep);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
int bodyIndicesOut[10];//MAX_SDF_BODIES = 10
int numJoints, numBodies;
int bodyUniqueId;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED);
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
ASSERT_EQ(numBodies,1);
bodyUniqueId = bodyIndicesOut[0];
{
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
{
struct b3VisualShapeInformation vi;
b3GetVisualShapeInformation(sm, &vi);
}
}
}
numJoints = b3GetNumJoints(sm,bodyUniqueId);
ASSERT_EQ(numJoints,7);
#if 0
b3Printf("numJoints: %d\n", numJoints);
for (i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo))
{
b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
}
}
#endif
{
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle commandHandle;
double jointAngle = 0.f;
int jointIndex;
commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
for (jointIndex=0;jointIndex<numJoints;jointIndex++)
{
b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName);
//setting the initial position, orientation and other arguments are optional
startPosX =2;
startPosY =0;
startPosZ = 1;
ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_URDF_LOADING_COMPLETED);
bodyIndex = b3GetStatusBodyIndex(statusHandle);
}
if (bodyIndex>=0)
{
numJoints = b3GetNumJoints(sm,bodyIndex);
for (i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
b3GetJointInfo(sm,bodyIndex, i,&jointInfo);
// printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
//pick the IMU link index based on torso name
if (strstr(jointInfo.m_linkName,"base_link"))
{
imuLinkIndex = i;
}
//pick the joint index based on joint name
if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
{
sensorJointIndexLeft = i;
}
if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
{
sensorJointIndexRight = i;
}
}
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0)
{
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
}
if (sensorJointIndexLeft>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
}
if(sensorJointIndexRight>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
}
{
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);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RIGID_BODY_CREATION_COMPLETED);
}
{
int statusType;
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_ACTUAL_STATE_UPDATE_COMPLETED);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
b3GetStatusActualState(statusHandle,
0, &posVarCount, &dofCount,
0, 0, 0, 0);
ASSERT_EQ(posVarCount,15);
ASSERT_EQ(dofCount,14);
}
}
{
#if 0
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
{
b3JointControlSetDesiredVelocity(command,dofIndex,1);
b3JointControlSetMaximumForce(command,dofIndex,100);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
#endif
}
///perform some simulation steps for testing
for ( i=0;i<100;i++)
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
if (b3CanSubmitCommand(sm))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
statusType = b3GetStatusType(statusHandle);
ASSERT_EQ(statusType, CMD_STEP_FORWARD_SIMULATION_COMPLETED);
} else
{
break;
}
}
if (b3CanSubmitCommand(sm))
{
b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex));
if (sensorJointIndexLeft>=0)
{
struct b3JointSensorState sensorState;
b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1],
sensorState.m_jointForceTorque[2]);
}
if (sensorJointIndexRight>=0)
{
struct b3JointSensorState sensorState;
b3GetJointState(sm,state,sensorJointIndexRight,&sensorState);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1],
sensorState.m_jointForceTorque[2]);
}
{
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED);
}
}
}
b3DisconnectSharedMemory(sm);
}
int main(int argc, char* argv[])
{
int i, dofCount , posVarCount, dofIndex, ret ,numJoints, allowSharedMemoryInitialization=0;
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.;
double startPosX, startPosY,startPosZ;
int imuLinkIndex = -1;
b3PhysicsClientHandle sm=0;
int bodyIndex = -1;
printf("hello world\n");
#ifdef PHYSICS_LOOP_BACK
sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
#else
sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
#endif
if (b3CanSubmitCommand(sm))
{
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz);
ret = b3PhysicsParamSetTimeStep(command, timeStep);
b3SubmitClientCommandAndWaitStatus(sm, command);
}
{
b3SharedMemoryStatusHandle statusHandle;
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);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
bodyIndex = b3GetStatusBodyIndex(statusHandle);
}
if (bodyIndex>=0)
{
numJoints = b3GetNumJoints(sm,bodyIndex);
for (i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
b3GetJointInfo(sm,bodyIndex, i,&jointInfo);
printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
//pick the IMU link index based on torso name
if (strstr(jointInfo.m_linkName,"base_link"))
{
imuLinkIndex = i;
}
//pick the joint index based on joint name
if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
{
sensorJointIndexLeft = i;
}
if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
{
sensorJointIndexRight = i;
}
}
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm);
b3SharedMemoryStatusHandle statusHandle;
if (imuLinkIndex>=0)
{
ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1);
}
if (sensorJointIndexLeft>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1);
}
if(sensorJointIndexRight>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1);
}
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);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
{
int statusType;
b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex);
b3SharedMemoryStatusHandle statusHandle;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
b3GetStatusActualState(statusHandle,
0, &posVarCount, &dofCount,
0, 0, 0, 0);
b3Printf("posVarCount = %d\n",posVarCount);
printf("dofCount = %d\n",dofCount);
}
}
{
#if 0
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY);
for ( dofIndex=0;dofIndex<dofCount;dofIndex++)
{
b3JointControlSetDesiredVelocity(command,dofIndex,1);
b3JointControlSetMaximumForce(command,dofIndex,100);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
#endif
}
///perform some simulation steps for testing
for ( i=0;i<100;i++)
{
b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm));
}
{
b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex));
if (sensorJointIndexLeft>=0)
{
struct b3JointSensorState sensorState;
b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1],
sensorState.m_jointForceTorque[2]);
}
if (sensorJointIndexRight>=0)
{
struct b3JointSensorState sensorState;
b3GetJointState(sm,state,sensorJointIndexRight,&sensorState);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1],
sensorState.m_jointForceTorque[2]);
}
}
{
b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm));
}
}
b3DisconnectSharedMemory(sm);
}
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;
const char* urdfFileName = "r2d2.urdf";
double gravx=0, gravy=0, gravz=-9.8;
double timeStep = 1./60.;
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);
if (b3CanSubmitCommand(sm))
{
ret = b3InitPhysicsParamCommand(&command);
ret = b3PhysicsParamSetGravity(&command, gravx,gravy, gravz);
ret = b3PhysicsParamSetTimeStep(&command, timeStep);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==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);
numJoints = b3GetNumJoints(sm);
for (i=0;i<numJoints;i++)
{
struct b3JointInfo jointInfo;
b3GetJointInfo(sm,i,&jointInfo);
printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
//pick the IMU link index based on torso name
if (strstr(jointInfo.m_linkName,"base_link"))
{
imuLinkIndex = i;
}
//pick the joint index based on joint name
if (strstr(jointInfo.m_jointName,"base_to_left_leg"))
{
sensorJointIndexLeft = i;
}
if (strstr(jointInfo.m_jointName,"base_to_right_leg"))
{
sensorJointIndexRight = i;
}
}
if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0))
{
ret = b3CreateSensorCommandInit(&command);
if (imuLinkIndex>=0)
{
ret = b3CreateSensorEnableIMUForLink(&command, imuLinkIndex, 1);
}
if (sensorJointIndexLeft>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1);
}
if(sensorJointIndexRight>=0)
{
ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexRight, 1);
}
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==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) {}
posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU;
b3Printf("posVarCount = %d\n",posVarCount);
printf("dofCount = %d\n",dofCount);
b3JointControlCommandInit(&command, 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, &status)==0) {}
///perform some simulation steps for testing
for ( i=0;i<100;i++)
{
ret = b3InitStepSimulationCommand(&command);
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) {}
if (sensorJointIndexLeft>=0)
{
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft,
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+0],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+1],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+2]);
}
if (sensorJointIndexRight>=0)
{
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+0],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1],
status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]);
}
ret = b3InitResetSimulationCommand(&command);
ret = b3SubmitClientCommand(sm, &command);
timeout = MAX_TIMEOUT;
while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {}
}
b3DisconnectSharedMemory(sm);
}