From 5517cbc4e016798289f8ebf835a650706a6490ee Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 28 Dec 2017 12:37:07 -0800 Subject: [PATCH] add segmask_linkindex.py example using p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX) to extract object unique id and link index from segmentation mask in getCameraImage --- examples/SharedMemory/PhysicsClientC_API.cpp | 14 +++++++ examples/SharedMemory/PhysicsClientC_API.h | 3 ++ .../SharedMemory/PhysicsClientExample.cpp | 2 +- .../PhysicsServerCommandProcessor.cpp | 7 +++- .../SharedMemory/PhysicsServerExample.cpp | 2 +- examples/SharedMemory/SharedMemoryCommands.h | 3 ++ examples/SharedMemory/SharedMemoryPublic.h | 4 ++ .../TinyRendererVisualShapeConverter.cpp | 21 +++++++++- .../TinyRendererVisualShapeConverter.h | 1 + examples/TinyRenderer/TinyRenderer.cpp | 4 +- examples/TinyRenderer/our_gl.cpp | 9 +--- .../pybullet/examples/saveRestoreState.py | 27 ++++++------ .../pybullet/examples/segmask_linkindex.py | 42 +++++++++++++++++++ examples/pybullet/pybullet.c | 13 ++++-- 14 files changed, 120 insertions(+), 32 deletions(-) create mode 100644 examples/pybullet/examples/segmask_linkindex.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 5efa52f08..cacd375a0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2940,6 +2940,20 @@ B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandl } } +B3_SHARED_API void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA); + if(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA) + { + command->m_requestPixelDataArguments.m_flags = flags; + command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_FLAGS; + } +} + + + B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 7aaabba87..d1d76815f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -209,6 +209,9 @@ B3_SHARED_API void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryComman B3_SHARED_API void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff); B3_SHARED_API void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow); B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer); +B3_SHARED_API void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags); + + B3_SHARED_API void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData); ///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 08adb66c6..355f5b9c5 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -940,7 +940,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) btVector4(32,255,255,255)}; if (segmentationMask>=0) { - int obIndex = segmentationMask&(0x1e24-1); + int obIndex = segmentationMask&((1<<24)-1); int linkIndex = (segmentationMask>>24)-1; btVector4 rgb = palette[(obIndex+linkIndex)&3]; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f9a6f20a5..b3d24b3e6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3017,7 +3017,12 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, clientCmd.m_requestPixelDataArguments.m_pixelHeight); } - + int flags = 0; + if (clientCmd.m_updateFlags&REQUEST_PIXEL_ARGS_HAS_FLAGS) + { + flags = clientCmd.m_requestPixelDataArguments.m_flags; + } + m_data->m_visualConverter.setFlags(flags); int numTotalPixels = width*height; int numRemainingPixels = numTotalPixels - startPixelIndex; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 8d13bf105..2a53c58e5 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -2169,7 +2169,7 @@ void PhysicsServerExample::updateGraphics() btVector4(32,255,255,255)}; if (segmentationMask>=0) { - int obIndex = segmentationMask&(0x1e24-1); + int obIndex = segmentationMask&((1<<24)-1); int linkIndex = (segmentationMask>>24)-1; btVector4 rgb = palette[(obIndex+linkIndex)&3]; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 3f6864ba8..e953104a7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -238,6 +238,7 @@ struct RequestPixelDataArgs float m_lightDiffuseCoeff; float m_lightSpecularCoeff; int m_hasShadow; + int m_flags; }; enum EnumRequestPixelDataUpdateFlags @@ -251,6 +252,8 @@ enum EnumRequestPixelDataUpdateFlags REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF=64, REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF=128, REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF=256, + REQUEST_PIXEL_ARGS_HAS_FLAGS = 512, + //don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index bb9bd8be1..b152a70c3 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -584,6 +584,10 @@ enum EnumRenderer //ER_FIRE_RAYS=(1<<18), }; +enum EnumRendererAuxFlags +{ + ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX=1, +}; ///flags to pick the IK solver and other options enum EnumCalculateInverseKinematicsFlags { diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index c687732da..d0a9c321a 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -82,6 +82,7 @@ struct TinyRendererVisualShapeConverterInternalData float m_lightSpecularCoeff; bool m_hasLightSpecularCoeff; bool m_hasShadow; + int m_flags; SimpleCamera m_camera; @@ -102,7 +103,8 @@ struct TinyRendererVisualShapeConverterInternalData m_hasLightDiffuseCoeff(false), m_lightSpecularCoeff(0.05), m_hasLightSpecularCoeff(false), - m_hasShadow(false) + m_hasShadow(false), + m_flags(0) { m_depthBuffer.resize(m_swWidth*m_swHeight); m_shadowBuffer.resize(m_swWidth*m_swHeight); @@ -154,6 +156,11 @@ void TinyRendererVisualShapeConverter::setShadow(bool hasShadow) { m_data->m_hasShadow = hasShadow; } +void TinyRendererVisualShapeConverter::setFlags(int flags) +{ + m_data->m_flags = flags; +} + void TinyRendererVisualShapeConverter::setLightAmbientCoeff(float ambientCoeff) { @@ -1043,7 +1050,17 @@ void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixels } if (segmentationMaskBuffer) { - segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex]; + int segMask = m_data->m_segmentationMaskBuffer[i + startPixelIndex]; + if ((m_data->m_flags & ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)==0) + { + //if we don't explicitly request link index, clear it out + //object index are the lower 24bits + if (segMask >= 0) + { + segMask &= ((1 << 24) - 1); + } + } + segmentationMaskBuffer[i] = segMask; } if (pixelsRGBA) diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.h b/examples/SharedMemory/TinyRendererVisualShapeConverter.h index 97bbb0a36..4c5abbfbb 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.h +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.h @@ -43,6 +43,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter void setLightDiffuseCoeff(float diffuseCoeff); void setLightSpecularCoeff(float specularCoeff); void setShadow(bool hasShadow); + void setFlags(int flags); void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels, int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied); diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index dbf6522e8..824791d55 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -562,12 +562,12 @@ void TinyRenderer::renderObject(TinyRenderObjectData& renderData) { for (int t=0;t &clipc, IShader &shader, TGAImage &image, float *zb zbuffer[P.x+P.y*image.get_width()] = frag_depth; if (segmentationMaskBuffer) { - if (objectAndLinkIndex) - { - segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex; - } else - { - segmentationMaskBuffer[P.x+P.y*image.get_width()] = 0; - - } + segmentationMaskBuffer[P.x+P.y*image.get_width()] = objectAndLinkIndex; } image.set(P.x, P.y, color); } diff --git a/examples/pybullet/examples/saveRestoreState.py b/examples/pybullet/examples/saveRestoreState.py index 0361bedba..cffc4916b 100644 --- a/examples/pybullet/examples/saveRestoreState.py +++ b/examples/pybullet/examples/saveRestoreState.py @@ -9,21 +9,22 @@ for i in range (10): p.setGravity(0,0,-10) -for i in range (500): - p.stepSimulation() +#for i in range (500): +# p.stepSimulation() + + + +saveState = 1 + +if saveState: + for i in range (500): + p.stepSimulation() + p.saveBullet("state.bullet") +else: + p.restoreState(fileName="state.bullet") for i in range (10): print("pos[",i,"]=",p.getBasePositionAndOrientation(i)) - -#saveState = 0 - -#if saveState: -# for i in range (500): -# p.stepSimulation() -# p.saveBullet("state.bullet") -#else: -# p.restoreState(fileName="state.bullet") - - + while (p.getConnectionInfo()["isConnected"]): time.sleep(1) \ No newline at end of file diff --git a/examples/pybullet/examples/segmask_linkindex.py b/examples/pybullet/examples/segmask_linkindex.py new file mode 100644 index 000000000..0c6ad2ff7 --- /dev/null +++ b/examples/pybullet/examples/segmask_linkindex.py @@ -0,0 +1,42 @@ +import pybullet as p +p.connect(p.GUI) +r2d2 = p.loadURDF("r2d2.urdf",[0,0,1]) +for l in range (p.getNumJoints(r2d2)): + print(p.getJointInfo(r2d2,l)) + +p.loadURDF("r2d2.urdf",[2,0,1]) +p.loadURDF("r2d2.urdf",[4,0,1]) + +p.getCameraImage(320,200,flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX) +segLinkIndex=1 +verbose = 0 + +while (1): + keys = p.getKeyboardEvents() + #for k in keys: + # print("key=",k,"state=", keys[k]) + if ord('1') in keys: + state = keys[ord('1')] + if (state & p.KEY_WAS_RELEASED): + verbose = 1-verbose + if ord('s') in keys: + state = keys[ord('s')] + if (state & p.KEY_WAS_RELEASED): + segLinkIndex = 1-segLinkIndex + #print("segLinkIndex=",segLinkIndex) + flags = 0 + if (segLinkIndex): + flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX + + img = p.getCameraImage(320,200,flags=flags) + #print(img[0],img[1]) + seg=img[4] + if (verbose): + for pixel in seg: + if (pixel>=0): + obUid = pixel & ((1<<24)-1) + linkIndex = (pixel >> 24)-1 + print("obUid=",obUid,"linkIndex=",linkIndex) + + + p.stepSimulation() \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d2cfe7636..8e5370594 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6046,16 +6046,16 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec float lightAmbientCoeff = -1; float lightDiffuseCoeff = -1; float lightSpecularCoeff = -1; - + int flags = -1; int renderer = -1; // inialize cmd b3SharedMemoryCommandHandle command; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; // set camera resolution, optionally view, projection matrix, light direction, light color, light distance, shadow - static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "physicsClientId", NULL}; + static char* kwlist[] = {"width", "height", "viewMatrix", "projectionMatrix", "lightDirection", "lightColor", "lightDistance", "shadow", "lightAmbientCoeff", "lightDiffuseCoeff", "lightSpecularCoeff", "renderer", "flags", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|OOOOfifffiii", kwlist, &width, &height, &objViewMat, &objProjMat, &lightDirObj, &lightColorObj, &lightDist, &hasShadow, &lightAmbientCoeff, &lightDiffuseCoeff, &lightSpecularCoeff, &renderer, &flags, &physicsClientId)) { return NULL; } @@ -6107,6 +6107,10 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec b3RequestCameraImageSetLightSpecularCoeff(command, lightSpecularCoeff); } + if (flags >= 0) + { + b3RequestCameraImageSetFlags(command, flags); + } if (renderer>=0) { b3RequestCameraImageSelectRenderer(command, renderer);//renderer could be ER_BULLET_HARDWARE_OPENGL @@ -8365,7 +8369,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER); PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL); - + PyModule_AddIntConstant(m, "ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX", ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX); + PyModule_AddIntConstant(m, "IK_DLS", IK_DLS); PyModule_AddIntConstant(m, "IK_SDLS", IK_SDLS); PyModule_AddIntConstant(m, "IK_HAS_TARGET_POSITION", IK_HAS_TARGET_POSITION);