void PhysicsServerExample::stepSimulation(float deltaTime)
{
if (m_replay)
{
for (int i=0;i<100;i++)
m_physicsServer.processClientCommands();
} else
{
btClock rtc;
btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
while (rtc.getTimeMilliseconds()<endTime)
{
m_physicsServer.processClientCommands();
}
}
}
void RobotControlExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
if (m_physicsClient.isConnected())
{
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumJoints();i++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
}
}
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
SharedMemoryCommand& cmd = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
m_physicsClient.submitClientCommand(cmd);
}
}
}
}
void PhysicsServerExample::stepSimulation(float deltaTime)
{
btClock rtc;
btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
while (rtc.getTimeMilliseconds()<endTime)
{
m_physicsServer.processClientCommands();
}
}
void PhysicsServerExample::stepSimulation(float deltaTime)
{
//this->m_physicsServer.processClientCommands();
//check if any graphics related tasks are requested
switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
{
case eGUIHelperCreateCollisionShapeGraphicsObject:
{
m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_multiThreadedHelper->m_colShape);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperCreateCollisionObjectGraphicsObject:
{
m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_multiThreadedHelper->m_obj,
m_multiThreadedHelper->m_color2);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperCreateRigidBodyGraphicsObject:
{
m_multiThreadedHelper->m_childGuiHelper->createRigidBodyGraphicsObject(m_multiThreadedHelper->m_body,m_multiThreadedHelper->m_color3);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperRegisterTexture:
{
m_multiThreadedHelper->m_textureId = m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_multiThreadedHelper->m_texels,
m_multiThreadedHelper->m_textureWidth,m_multiThreadedHelper->m_textureHeight);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperRegisterGraphicsShape:
{
m_multiThreadedHelper->m_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
m_multiThreadedHelper->m_vertices,
m_multiThreadedHelper->m_numvertices,
m_multiThreadedHelper->m_indices,
m_multiThreadedHelper->m_numIndices,
m_multiThreadedHelper->m_primitiveType,
m_multiThreadedHelper->m_textureId);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperRegisterGraphicsInstance:
{
m_multiThreadedHelper->m_instanceId = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance(
m_multiThreadedHelper->m_shapeIndex,
m_multiThreadedHelper->m_position,
m_multiThreadedHelper->m_quaternion,
m_multiThreadedHelper->m_color,
m_multiThreadedHelper->m_scaling);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperRemoveAllGraphicsInstances:
{
m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
b3Assert(numRenderInstances==0);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperCopyCameraImageData:
{
m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix,
m_multiThreadedHelper->m_projectionMatrix,
m_multiThreadedHelper->m_pixelsRGBA,
m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
m_multiThreadedHelper->m_depthBuffer,
m_multiThreadedHelper->m_depthBufferSizeInPixels,
m_multiThreadedHelper->m_segmentationMaskBuffer,
m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
m_multiThreadedHelper->m_startPixelIndex,
m_multiThreadedHelper->m_destinationWidth,
m_multiThreadedHelper->m_destinationHeight,
m_multiThreadedHelper->m_numPixelsCopied);
m_multiThreadedHelper->getCriticalSection()->lock();
m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eGUIHelperIdle:
default:
{
}
}
#if 0
if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
{
btClock rtc;
btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
while (rtc.getTimeMilliseconds()<endTime)
{
m_physicsServer.processClientCommands();
}
} else
{
//for (int i=0;i<10;i++)
m_physicsServer.processClientCommands();
}
#endif
{
if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
{
m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
}
}
}
void RobotControlExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
if (m_physicsClient.isConnected())
{
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumJoints();i++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(i,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);
}
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
switch (m_option)
{
case ROBOT_VELOCITY_CONTROL:
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
break;
}
case ROBOT_PD_CONTROL:
{
char motorName[1024];
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_kp = 1;
motorInfo->m_kd = 0;
{
sprintf(motorName,"%s kp", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kp);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_posTarget);
slider.m_minVal=-SIMD_PI;
slider.m_maxVal=SIMD_PI;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s kd", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kd);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q'", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-10;
slider.m_maxVal=10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
break;
}
default:
{
b3Warning("Unknown control mode in RobotControlExample::stepSimulation");
}
};
}
}
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
if (m_verboseOutput)
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
}
SharedMemoryCommand cmd = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
{
char relativeFileName[1024];
bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
if (fileFound)
{
FILE *fp = fopen(relativeFileName, "rb");
if (fp)
{
fseek(fp, 0L, SEEK_END);
int mFileLen = ftell(fp);
fseek(fp, 0L, SEEK_SET);
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
char* data = (char*)malloc(mFileLen);
fread(data, mFileLen, 1, fp);
fclose(fp);
cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
if (m_verboseOutput)
{
b3Printf("Loaded bullet data chunks into shared memory\n");
}
free(data);
} else
{
b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
} else
{
b3Warning("Cannot open file %s\n", relativeFileName);
}
} else
{
b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
}
}
m_physicsClient.submitClientCommand(cmd);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
}
}
}
}
}
void RobotControlExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
if (m_physicsClient.isConnected())
{
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (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);
}
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
switch (m_option)
{
case ROBOT_VELOCITY_CONTROL:
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_jointName = info.m_jointName;
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
break;
}
case ROBOT_PD_CONTROL:
{
char motorName[1024];
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_jointName = info.m_jointName;
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_kp = 1;
motorInfo->m_kd = 0;
{
sprintf(motorName,"%s kp", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kp);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_posTarget);
slider.m_minVal=-SIMD_PI;
slider.m_maxVal=SIMD_PI;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s kd", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kd);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q'", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-10;
slider.m_maxVal=10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
break;
}
case ROBOT_PING_PONG_JOINT_FEEDBACK:
{
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_jointName = info.m_jointName;
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_jointIndex = jointIndex;
sensorCommand.m_createSensorArguments.m_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex;
sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true;
sensorCommand.m_createSensorArguments.m_numJointSensorChanges++;
m_numMotors++;
}
}
break;
}
default:
{
b3Warning("Unknown control mode in RobotControlExample::stepSimulation");
}
};
}
}
}
if (sensorCommand.m_createSensorArguments.m_numJointSensorChanges)
{
enqueueCommand(sensorCommand);
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
if (m_verboseOutput)
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
}
SharedMemoryCommand cmd = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
if (cmd.m_type == CMD_CREATE_SENSOR)
{
b3Printf("CMD_CREATE_SENSOR!\n");
}
if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
{
char relativeFileName[1024];
bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
if (fileFound)
{
FILE *fp = fopen(relativeFileName, "rb");
if (fp)
{
fseek(fp, 0L, SEEK_END);
int mFileLen = ftell(fp);
fseek(fp, 0L, SEEK_SET);
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
char* data = (char*)malloc(mFileLen);
fread(data, mFileLen, 1, fp);
fclose(fp);
cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
if (m_verboseOutput)
{
b3Printf("Loaded bullet data chunks into shared memory\n");
}
free(data);
} else
{
b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
} else
{
b3Warning("Cannot open file %s\n", relativeFileName);
}
} else
{
b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
}
}
m_physicsClient.submitClientCommand(cmd);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
command.m_type = CMD_REQUEST_ACTUAL_STATE;
enqueueCommand(command);
}
}
}
}
}
void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_options == eCLIENTEXAMPLE_SERVER)
{
for (int i=0;i<100;i++)
{
m_physicsServer.processClientCommands();
}
}
if (m_prevSelectedBody != m_selectedBody)
{
createButtons();
m_prevSelectedBody = m_selectedBody;
}
//while (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//b3Printf("bla\n");
}
if (statusType ==CMD_CAMERA_IMAGE_COMPLETED)
{
static int counter=0;
char msg[1024];
sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle,&imageData);
if (m_canvas && m_canvasIndex >=0)
{
for (int i=0;i<imageData.m_pixelWidth;i++)
{
for (int j=0;j<imageData.m_pixelHeight;j++)
{
int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth)));
int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight)));
btClamp(yIndex,0,imageData.m_pixelHeight);
btClamp(xIndex,0,imageData.m_pixelWidth);
int bytesPerPixel = 4;
int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel;
m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex,
imageData.m_rgbColorData[pixelIndex],
imageData.m_rgbColorData[pixelIndex+1],
imageData.m_rgbColorData[pixelIndex+2],
255);
// imageData.m_rgbColorData[pixelIndex+3]);
}
}
m_canvas->refreshImageData(m_canvasIndex);
}
b3Printf(msg);
}
if (statusType == CMD_CAMERA_IMAGE_FAILED)
{
b3Printf("Camera image FAILED\n");
}
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
int bodyIndex = b3GetStatusBodyIndex(status);
if (bodyIndex>=0)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
ComboBoxParams comboParams;
comboParams.m_comboboxId = bodyIndex;
comboParams.m_numItems = 1;
comboParams.m_startItem = 0;
comboParams.m_callback = MyComboBoxCallback;
comboParams.m_userPointer = this;
const char* bla = "bla";
const char* blarray[1];
blarray[0] = bla;
comboParams.m_items=blarray;//{&bla};
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
}
}
}
}
if (b3CanSubmitCommand(m_physicsClientHandle))
{
if (m_userCommandRequests.size())
{
//b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
int commandId = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
//for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders
if (commandId ==CMD_RESET_SIMULATION)
{
m_selectedBody = -1;
m_numMotors=0;
createButtons();
b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle);
if (m_options == eCLIENTEXAMPLE_SERVER)
{
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
while (!b3CanSubmitCommand(m_physicsClientHandle))
{
m_physicsServer.processClientCommands();
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
b3Printf("Status after reset: %d",statusType);
}
}
} else
{
prepareAndSubmitCommand(commandId);
}
} else
{
prepareAndSubmitCommand(commandId);
}
} else
{
if (m_numMotors)
{
enqueueCommand(CMD_SEND_DESIRED_STATE);
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
if (m_options != eCLIENTEXAMPLE_SERVER)
{
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
}
//enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
}
}
}
}
void PhysicsServerExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
}
void PhysicsClientExample::stepSimulation(float deltaTime)
{
if (m_options == eCLIENTEXAMPLE_SERVER)
{
for (int i = 0; i < 100; i++)
{
m_physicsServer.processClientCommands();
}
}
if (m_prevSelectedBody != m_selectedBody)
{
createButtons();
m_prevSelectedBody = m_selectedBody;
}
//while (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
bool hasStatus = (status != 0);
if (hasStatus)
{
int statusType = b3GetStatusType(status);
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//b3Printf("bla\n");
}
if (statusType == CMD_CAMERA_IMAGE_COMPLETED)
{
// static int counter=0;
// char msg[1024];
// sprintf(msg,"Camera image %d OK\n",counter++);
b3CameraImageData imageData;
b3GetCameraImageData(m_physicsClientHandle, &imageData);
if (m_canvas)
{
//compute depth image range
float minDepthValue = 1e20f;
float maxDepthValue = -1e20f;
for (int i = 0; i < camVisualizerWidth; i++)
{
for (int j = 0; j < camVisualizerHeight; j++)
{
int xIndex = int(float(i) * (float(imageData.m_pixelWidth) / float(camVisualizerWidth)));
int yIndex = int(float(j) * (float(imageData.m_pixelHeight) / float(camVisualizerHeight)));
btClamp(xIndex, 0, imageData.m_pixelWidth);
btClamp(yIndex, 0, imageData.m_pixelHeight);
if (m_canvasDepthIndex >= 0)
{
int depthPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth);
float depthValue = imageData.m_depthValues[depthPixelIndex];
//todo: rescale the depthValue to [0..255]
if (depthValue > -1e20)
{
maxDepthValue = btMax(maxDepthValue, depthValue);
minDepthValue = btMin(minDepthValue, depthValue);
}
}
}
}
for (int i = 0; i < camVisualizerWidth; i++)
{
for (int j = 0; j < camVisualizerHeight; j++)
{
int xIndex = int(float(i) * (float(imageData.m_pixelWidth) / float(camVisualizerWidth)));
int yIndex = int(float(j) * (float(imageData.m_pixelHeight) / float(camVisualizerHeight)));
btClamp(yIndex, 0, imageData.m_pixelHeight);
btClamp(xIndex, 0, imageData.m_pixelWidth);
int bytesPerPixel = 4; //RGBA
if (m_canvasRGBIndex >= 0)
{
int rgbPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth) * bytesPerPixel;
m_canvas->setPixel(m_canvasRGBIndex, i, j,
imageData.m_rgbColorData[rgbPixelIndex],
imageData.m_rgbColorData[rgbPixelIndex + 1],
imageData.m_rgbColorData[rgbPixelIndex + 2],
255); //alpha set to 255
}
if (m_canvasDepthIndex >= 0)
{
int depthPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth);
float depthValue = imageData.m_depthValues[depthPixelIndex];
//todo: rescale the depthValue to [0..255]
if (depthValue > -1e20)
{
int rgb = 0;
if (maxDepthValue != minDepthValue)
{
rgb = (depthValue - minDepthValue) * (255. / (btFabs(maxDepthValue - minDepthValue)));
if (rgb < 0 || rgb > 255)
{
//printf("rgb=%d\n",rgb);
}
}
m_canvas->setPixel(m_canvasDepthIndex, i, j,
rgb,
rgb,
255, 255); //alpha set to 255
}
else
{
m_canvas->setPixel(m_canvasDepthIndex, i, j,
0,
0,
0, 255); //alpha set to 255
}
}
if (m_canvasSegMaskIndex >= 0 && (0 != imageData.m_segmentationMaskValues))
{
int segmentationMaskPixelIndex = (xIndex + yIndex * imageData.m_pixelWidth);
int segmentationMask = imageData.m_segmentationMaskValues[segmentationMaskPixelIndex];
btVector4 palette[4] = {btVector4(32, 255, 32, 255),
btVector4(32, 32, 255, 255),
btVector4(255, 255, 32, 255),
btVector4(32, 255, 255, 255)};
if (segmentationMask >= 0)
{
int obIndex = segmentationMask & ((1 << 24) - 1);
int linkIndex = (segmentationMask >> 24) - 1;
btVector4 rgb = palette[(obIndex + linkIndex) & 3];
m_canvas->setPixel(m_canvasSegMaskIndex, i, j,
rgb.x(),
rgb.y(),
rgb.z(), 255); //alpha set to 255
}
else
{
m_canvas->setPixel(m_canvasSegMaskIndex, i, j,
0,
0,
0, 255); //alpha set to 255
}
}
}
}
if (m_canvasRGBIndex >= 0)
m_canvas->refreshImageData(m_canvasRGBIndex);
if (m_canvasDepthIndex >= 0)
m_canvas->refreshImageData(m_canvasDepthIndex);
if (m_canvasSegMaskIndex >= 0)
m_canvas->refreshImageData(m_canvasSegMaskIndex);
}
