Method stepSimulation

examples/SharedMemory/RobotControlExample.cpp:392–653

void	RobotControlExample::stepSimulation(float deltaTime)
{
    
	m_physicsServer.processClientCommands();

	if (m_physicsClient.isConnected())
    {
		
		SharedMemoryStatus status;
		bool hasStatus = m_physicsClient.processServerStatus(status);
		if ((m_option==ROBOT_PING_PONG_JOINT_FEEDBACK) && hasStatus && status.m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
		{
			//update sensor feedback: joint force/torque data and measured joint positions
			
			for (int i=0;i<m_numMotors;i++)
			{
				int jointIndex = m_motorTargetState[i].m_jointIndex;
				int positionIndex = m_motorTargetState[i].m_posIndex;
				int velocityIndex = m_motorTargetState[i].m_uIndex;

				m_motorTargetState[i].m_measuredJointPosition = status.m_sendActualStateArgs.m_actualStateQ[positionIndex];
				m_motorTargetState[i].m_measuredJointVelocity = status.m_sendActualStateArgs.m_actualStateQdot[velocityIndex];
				m_motorTargetState[i].m_measuredJointForce.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex],
																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+1],
																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+2]);
				m_motorTargetState[i].m_measuredJointTorque.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+3],
																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+4],
																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+5]);
				
				if (m_motorTargetState[i].m_measuredJointPosition>0.1)
				{
					m_motorTargetState[i].m_velTarget = -1.5;
				} else
				{
					m_motorTargetState[i].m_velTarget = 1.5;
				}
				
				b3Printf("Joint Force (Linear) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointForce.x(),m_motorTargetState[i].m_measuredJointForce.y(),m_motorTargetState[i].m_measuredJointForce.z());
				b3Printf("Joint Torque (Angular) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointTorque.x(),m_motorTargetState[i].m_measuredJointTorque.y(),m_motorTargetState[i].m_measuredJointTorque.z());

			}

			
		}

		if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
		{
			SharedMemoryCommand sensorCommand;
			sensorCommand.m_type = CMD_CREATE_SENSOR;
			sensorCommand.m_createSensorArguments.m_numJointSensorChanges = 0;

			for (int jointIndex=0;jointIndex<m_physicsClient.getNumJoints();jointIndex++)
			{
				b3JointInfo info;
				m_physicsClient.getJointInfo(jointIndex,info);
				if (m_verboseOutput)
				{
					b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);