PhysicsClientExample::~PhysicsClientExample()
{
if (m_physicsClientHandle)
{
b3ProcessServerStatus(m_physicsClientHandle);
b3DisconnectSharedMemory(m_physicsClientHandle);
}
if (m_options == eCLIENTEXAMPLE_SERVER)
{
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
if (m_canvas)
{
if (m_canvasRGBIndex >= 0)
m_canvas->destroyCanvas(m_canvasRGBIndex);
if (m_canvasDepthIndex >= 0)
m_canvas->destroyCanvas(m_canvasDepthIndex);
if (m_canvasSegMaskIndex >= 0)
m_canvas->destroyCanvas(m_canvasSegMaskIndex);
}
b3Printf("~PhysicsClientExample\n");
}
PhysicsClientExample::~PhysicsClientExample()
{
if (m_physicsClientHandle)
{
b3ProcessServerStatus(m_physicsClientHandle);
b3DisconnectSharedMemory(m_physicsClientHandle);
}
b3Printf("~PhysicsClientExample\n");
}
b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
{
int timeout = 1024*1024*1024;
b3SharedMemoryStatusHandle statusHandle=0;
b3SubmitClientCommand(physClient,commandHandle);
while ((statusHandle==0) && (timeout-- > 0))
{
statusHandle =b3ProcessServerStatus(physClient);
}
return (b3SharedMemoryStatusHandle) statusHandle;
}
PhysicsClientExample::~PhysicsClientExample()
{
if (m_physicsClientHandle)
{
b3ProcessServerStatus(m_physicsClientHandle);
b3DisconnectSharedMemory(m_physicsClientHandle);
}
if (m_options == eCLIENTEXAMPLE_SERVER)
{
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
b3Printf("~PhysicsClientExample\n");
}
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 PhysicsClientExample::stepSimulation(float deltaTime)
{
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_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();
}
prepareAndSubmitCommand(commandId);
} else
{
if (m_numMotors)
{
enqueueCommand(CMD_SEND_DESIRED_STATE);
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
//enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
}
}
}
}
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);
}
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);
}
