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);
}
}
}
}
static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
{
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
///request an image from a simulated camera, using a software renderer.
struct b3CameraImageData imageData;
PyObject* objViewMat,* objProjMat;
int width, height;
if (PyArg_ParseTuple(args, "iiOO", &width, &height, &objViewMat, &objProjMat))
{
PyObject* seq;
int i, len;
PyObject* item;
float viewMatrix[16];
float projectionMatrix[16];
int valid = 1;
{
seq = PySequence_Fast(objViewMat, "expected a sequence");
len = PySequence_Size(objViewMat);
//printf("objViewMat size = %d\n", len);
if (len==16)
{
if (PyList_Check(seq))
{
for (i = 0; i < len; i++)
{
item = PyList_GET_ITEM(seq, i);
viewMatrix[i] = PyFloat_AsDouble(item);
float v = viewMatrix[i];
//printf("view %d to %f\n", i,v);
}
}
else
{
for (i = 0; i < len; i++)
{
item = PyTuple_GET_ITEM(seq,i);
viewMatrix[i] = PyFloat_AsDouble(item);
}
}
} else
{
valid = 0;
}
}
{
seq = PySequence_Fast(objProjMat, "expected a sequence");
len = PySequence_Size(objProjMat);
//printf("projMat len = %d\n", len);
if (len==16)
{
if (PyList_Check(seq))
{
for (i = 0; i < len; i++)
{
item = PyList_GET_ITEM(seq, i);
projectionMatrix[i] = PyFloat_AsDouble(item);
}
}
else
{
for (i = 0; i < len; i++)
{
item = PyTuple_GET_ITEM(seq,i);
projectionMatrix[i] = PyFloat_AsDouble(item);
}
}
} else
{
valid = 0;
}
}
Py_DECREF(seq);
{
b3SharedMemoryCommandHandle command;
command = b3InitRequestCameraImage(sm);
if (valid)
{
//printf("set b3RequestCameraImageSetCameraMatrices\n");
b3RequestCameraImageSetCameraMatrices(command, viewMatrix, projectionMatrix);
}
b3RequestCameraImageSetPixelResolution(command,width,height);
if (b3CanSubmitCommand(sm))
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType==CMD_CAMERA_IMAGE_COMPLETED)
{
PyObject *item2;
PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
b3GetCameraImageData(sm, &imageData);
//todo: error handling if image size is 0
pyResultList = PyTuple_New(4);
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
PyObject *pylistPos;
PyObject* pylistDep;
int i,j,p;
//printf("image width = %d, height = %d\n", imageData.m_pixelWidth, imageData.m_pixelHeight);
{
PyObject *item;
int bytesPerPixel = 4;//Red, Green, Blue, each 8 bit values
int num=bytesPerPixel*imageData.m_pixelWidth*imageData.m_pixelHeight;
pylistPos = PyTuple_New(num);
pylistDep = PyTuple_New(imageData.m_pixelWidth*imageData.m_pixelHeight);
for (i=0;i<imageData.m_pixelWidth;i++)
{
for (j=0;j<imageData.m_pixelHeight;j++)
{
int depIndex = i+j*imageData.m_pixelWidth;
item = PyFloat_FromDouble(imageData.m_depthValues[depIndex]);
PyTuple_SetItem(pylistDep, depIndex, item);
for (p=0;p<bytesPerPixel;p++)
{
int pixelIndex = bytesPerPixel*(i+j*imageData.m_pixelWidth)+p;
item = PyInt_FromLong(imageData.m_rgbColorData[pixelIndex]);
PyTuple_SetItem(pylistPos, pixelIndex, item);
}
}
}
}
PyTuple_SetItem(pyResultList, 2,pylistPos);
PyTuple_SetItem(pyResultList, 3,pylistDep);
return pyResultList;
}
}
}
}
Py_INCREF(Py_None);
return Py_None;
}
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);
}
