Function testSharedMemory

test/SharedMemory/test.c:40–298

#endif

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);