From a9996088c81b8a632750db522470a5e6a3da408c Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 24 Feb 2019 14:09:42 -0800 Subject: [PATCH] Implement PyBullet.getCameraImage for PhysX backend. PhysX backend, allow arbitrary plane normal, a few other fixes. --- .../ImportURDFDemo/UrdfRenderingInterface.h | 7 + .../physx/PhysXServerCommandProcessor.cpp | 482 ++++++++++++++---- .../physx/PhysXServerCommandProcessor.h | 3 + examples/SharedMemory/physx/URDF2PhysX.cpp | 24 +- .../eglRendererVisualShapeConverter.cpp | 58 +++ .../eglRendererVisualShapeConverter.h | 3 + examples/pybullet/examples/testrender.py | 8 +- 7 files changed, 481 insertions(+), 104 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h index 7e01cffc7..02aa42534 100644 --- a/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h +++ b/examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h @@ -95,6 +95,13 @@ struct UrdfRenderingInterface virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]) {} virtual void setProjectiveTexture(bool useProjectiveTexture) {} + + + virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const + { + return false; + } + }; #endif //LINK_VISUAL_SHAPES_CONVERTER_H diff --git a/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp index 89c24e8a4..d5aa86666 100644 --- a/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp +++ b/examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp @@ -109,8 +109,9 @@ struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEvent btSpinMutex m_taskLock; btAlignedObjectArray m_contactPoints; - + + void onContactModify(physx::PxContactModifyPair* const pairs, physx::PxU32 count) { for (physx::PxU32 i = 0; i m_userDebugParameters; + btAlignedObjectArray m_graphicsIndexToSegmentationMask; PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk, int argc, char* argv[]) : m_isConnected(false), @@ -245,7 +249,8 @@ struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEvent m_pluginManager(sdk), m_profileTimingLoggingUid(-1), m_stateLoggersUniqueId(1), - m_commandLineArgs(argc,argv) + m_commandLineArgs(argc,argv), + m_userDebugParametersUid(0) { m_foundation = NULL; @@ -756,13 +761,14 @@ bool PhysXServerCommandProcessor::processSendDesiredStateCommand(const struct Sh //find the joint motors and apply the desired velocity and maximum force/torque { int dofIndex = 6; //skip the 3 linear + 3 angular degree of freedom entries of the base + int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base for (int link = 1; link < numLinks2; link++) { int dofs = physxLinks[link]->getInboundJointDof(); physx::PxReal stiffness = 10.f; physx::PxReal damping = 0.1f; physx::PxReal forceLimit = PX_MAX_F32; - int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base + if (dofs == 1) { @@ -782,33 +788,34 @@ bool PhysXServerCommandProcessor::processSendDesiredStateCommand(const struct Sh if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0) { desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; - } - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0) - { - kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; - } - physx::PxReal damping = kd; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KP) != 0) - { - kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex]; - stiffness = kp; - } + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex]; + } + physx::PxReal damping = kd; - joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity); - - + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KP) != 0) + { + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex]; + stiffness = kp; + } - physx::PxReal forceLimit = 1000000.f; - if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) - { - forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]; + joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity); + + + + physx::PxReal forceLimit = 1000000.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) + { + forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]; + } + bool isAcceleration = false; + + joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, desiredPosition); + joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit, isAcceleration); } - bool isAcceleration = false; - joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit, isAcceleration); - - joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, desiredPosition); } dofIndex += dofs; @@ -1262,6 +1269,46 @@ bool PhysXServerCommandProcessor::processSetAdditionalSearchPathCommand(const st return hasStatus; } +bool PhysXServerCommandProcessor::processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + ///dummy, so commands don't fail + bool hasStatus = true; + BT_PROFILE("CMD_USER_DEBUG_DRAW"); + SharedMemoryStatus& serverCmd = serverStatusOut; + int uid = 0; + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; + + if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER) + { + int uid = m_data->m_userDebugParametersUid++; + double value = clientCmd.m_userDebugDrawArgs.m_startValue; + m_data->m_userDebugParameters.insert(uid, value); + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags & USER_DEBUG_READ_PARAMETER) + { + double* valPtr = m_data->m_userDebugParameters[clientCmd.m_userDebugDrawArgs.m_itemUniqueId]; + if (valPtr) + { + serverCmd.m_userDebugDrawArgs.m_parameterValue = *valPtr; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED; + } + } + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL) + { + m_data->m_userDebugParameters.clear(); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) + { + m_data->m_userDebugParameters.remove(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + return hasStatus; + +} bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { @@ -1381,6 +1428,19 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman hasStatus = processSetAdditionalSearchPathCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } + + case CMD_USER_DEBUG_DRAW: + { + hasStatus = processUserDebugDrawCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_CAMERA_IMAGE_DATA: + { + hasStatus = processRequestCameraImageCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + #if 0 case CMD_SET_VR_CAMERA_STATE: { @@ -1415,11 +1475,6 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman break; } - case CMD_REQUEST_CAMERA_IMAGE_DATA: - { - hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } case CMD_REQUEST_BODY_INFO: { @@ -1553,45 +1608,45 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman break; } case CMD_REMOVE_BODY: - { - hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processRemoveBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_USER_CONSTRAINT: - { - hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processCreateUserConstraintCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_CALCULATE_INVERSE_KINEMATICS: - { - hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processCalculateInverseKinematicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_REQUEST_VISUAL_SHAPE_INFO: - { - hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processRequestVisualShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_REQUEST_COLLISION_SHAPE_INFO: { hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); break; } case CMD_UPDATE_VISUAL_SHAPE: - { - hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processUpdateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_CHANGE_TEXTURE: - { - hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processChangeTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_LOAD_TEXTURE: - { - hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processLoadTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_RESTORE_STATE: { hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); @@ -1605,56 +1660,291 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman } case CMD_LOAD_BULLET: - { - hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processLoadBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_SAVE_BULLET: - { - hasStatus = processSaveBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processSaveBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_LOAD_MJCF: - { - hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } - case CMD_USER_DEBUG_DRAW: - { - hasStatus = processUserDebugDrawCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } - + { + hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_USER_DATA: - { - hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processRequestUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_ADD_USER_DATA: - { - hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processAddUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } case CMD_REMOVE_USER_DATA: - { - hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); - break; - } + { + hasStatus = processRemoveUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } #endif - default: - { - BT_PROFILE("CMD_UNKNOWN"); - printf("Unknown command encountered: %d", clientCmd.m_type); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; - hasStatus = true; - } + default: + { + BT_PROFILE("CMD_UNKNOWN"); + printf("Unknown command encountered: %d", clientCmd.m_type); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; + hasStatus = true; + } }; return hasStatus; } + + +bool PhysXServerCommandProcessor::processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA"); + int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex; + int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth; + int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight; + int numPixelsCopied = 0; + + int oldWidth; + int oldHeight; + m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(oldWidth, oldHeight); + + serverStatusOut.m_type = CMD_CAMERA_IMAGE_FAILED; + + if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) && + (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT) != 0) + { + if (m_data->m_pluginManager.getRenderInterface()) + { + + m_data->m_pluginManager.getRenderInterface()->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; + } + if (m_data->m_pluginManager.getRenderInterface()) + { + m_data->m_pluginManager.getRenderInterface()->setFlags(flags); + } + + int numTotalPixels = width * height; + int numRemainingPixels = numTotalPixels - startPixelIndex; + + if (numRemainingPixels > 0) + { + int totalBytesPerPixel = 4 + 4 + 4; //4 for rgb, 4 for depth, 4 for segmentation mask + int maxNumPixels = bufferSizeInBytes / totalBytesPerPixel - 1; + unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; + int numRequestedPixels = btMin(maxNumPixels, numRemainingPixels); + + float* depthBuffer = (float*)(bufferServerToClient + numRequestedPixels * 4); + int* segmentationMaskBuffer = (int*)(bufferServerToClient + numRequestedPixels * 8); + + serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel; + float viewMat[16]; + float projMat[16]; + float projTextureViewMat[16]; + float projTextureProjMat[16]; + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) == 0) + { + b3OpenGLVisualizerCameraInfo tmpCamResult; + bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo( + &tmpCamResult.m_width, + &tmpCamResult.m_height, + tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix, + tmpCamResult.m_camUp, + tmpCamResult.m_camForward, + tmpCamResult.m_horizontal, + tmpCamResult.m_vertical, + &tmpCamResult.m_yaw, + &tmpCamResult.m_pitch, + &tmpCamResult.m_dist, + tmpCamResult.m_target); + if (result) + { + for (int i = 0; i < 16; i++) + { + viewMat[i] = tmpCamResult.m_viewMatrix[i]; + projMat[i] = tmpCamResult.m_projectionMatrix[i]; + } + } + else + { + //failed + m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight); + return hasStatus; + } + } + else + { + for (int i = 0; i < 16; i++) + { + viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i]; + projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i]; + } + } + + { + if (m_data->m_pluginManager.getRenderInterface()) + { + if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) + { + // printf("-------------------------------\nRendering\n"); + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow != 0)); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff); + } + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff); + } + + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0) + { + m_data->m_pluginManager.getRenderInterface()->render( + clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix); + } + else + { + b3OpenGLVisualizerCameraInfo tmpCamResult; + bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo( + &tmpCamResult.m_width, + &tmpCamResult.m_height, + tmpCamResult.m_viewMatrix, + tmpCamResult.m_projectionMatrix, + tmpCamResult.m_camUp, + tmpCamResult.m_camForward, + tmpCamResult.m_horizontal, + tmpCamResult.m_vertical, + &tmpCamResult.m_yaw, + &tmpCamResult.m_pitch, + &tmpCamResult.m_dist, + tmpCamResult.m_target); + if (result) + { + m_data->m_pluginManager.getRenderInterface()->render(tmpCamResult.m_viewMatrix, tmpCamResult.m_projectionMatrix); + } + else + { + m_data->m_pluginManager.getRenderInterface()->render(); + } + } + } + } + + if (m_data->m_pluginManager.getRenderInterface()) + { + if ((flags & ER_USE_PROJECTIVE_TEXTURE) != 0) + { + m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(true); + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES) != 0) + { + for (int i = 0; i < 16; i++) + { + projTextureViewMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i]; + projTextureProjMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i]; + } + } + else // If no specified matrices for projective texture, then use the camera matrices. + { + for (int i = 0; i < 16; i++) + { + projTextureViewMat[i] = viewMat[i]; + projTextureProjMat[i] = projMat[i]; + } + } + m_data->m_pluginManager.getRenderInterface()->setProjectiveTextureMatrices(projTextureViewMat, projTextureProjMat); + } + else + { + m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false); + } + + if ((flags & ER_NO_SEGMENTATION_MASK) != 0) + { + segmentationMaskBuffer = 0; + } + + m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels, + depthBuffer, numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex, &width, &height, &numPixelsCopied); + m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false); + } + + #if 0 + m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix, + clientCmd.m_requestPixelDataArguments.m_projectionMatrix, pixelRGBA, numRequestedPixels, + depthBuffer, numRequestedPixels, + segmentationMaskBuffer, numRequestedPixels, + startPixelIndex, width, height, &numPixelsCopied); + #endif + } + + //each pixel takes 4 RGBA values and 1 float = 8 bytes + } + else + { + } + + m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight); + serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED; + + serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied; + serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied; + serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex; + serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width; + serverStatusOut.m_sendPixelDataArguments.m_imageHeight = height; + return hasStatus; + +} + bool PhysXServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; diff --git a/examples/SharedMemory/physx/PhysXServerCommandProcessor.h b/examples/SharedMemory/physx/PhysXServerCommandProcessor.h index f50ad44b1..7566268e2 100644 --- a/examples/SharedMemory/physx/PhysXServerCommandProcessor.h +++ b/examples/SharedMemory/physx/PhysXServerCommandProcessor.h @@ -23,11 +23,14 @@ class PhysXServerCommandProcessor : public PhysicsCommandProcessorInterface bool processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); void resetSimulation(); bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + public: PhysXServerCommandProcessor(int argc, char* argv[]); diff --git a/examples/SharedMemory/physx/URDF2PhysX.cpp b/examples/SharedMemory/physx/URDF2PhysX.cpp index 31f2db41d..2a5a19a87 100644 --- a/examples/SharedMemory/physx/URDF2PhysX.cpp +++ b/examples/SharedMemory/physx/URDF2PhysX.cpp @@ -281,11 +281,21 @@ int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedDat { btVector3 planeNormal = col.m_geometry.m_planeNormal; btScalar planeConstant = 0; //not available? - //PxPlane(1, 0, 0, 0). + + btVector3 planeRefAxis(1, 0, 0); + btQuaternion diffQuat = shortestArcQuat(planeRefAxis, planeNormal); shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxPlaneGeometry(), *material); - //todo: compensate for plane axis - //physx::PxTransform localPose = tr*physx::PxTransform - //shape->setLocalPose(localPose); + + btTransform diffTr; + diffTr.setIdentity(); + diffTr.setRotation(diffQuat); + btTransform localTrans = localInertiaFrame.inverse()*childTrans*diffTr; + physx::PxTransform tr; + tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]); + tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]); + + physx::PxTransform localPose = tr; + shape->setLocalPose(localPose); numShapes++; break; } @@ -698,8 +708,8 @@ btTransform ConvertURDF2PhysXInternal( //Now create the slider and fixed joints... - cache.m_articulation->setSolverIterationCounts(4);//todo: API? - //cache.m_articulation->setSolverIterationCounts(32);//todo: API? + //cache.m_articulation->setSolverIterationCounts(4);//todo: API? + cache.m_articulation->setSolverIterationCounts(32);//todo: API? cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED); cache.m_parentLocalPoses.push_back(physx::PxTransform()); @@ -815,7 +825,7 @@ btTransform ConvertURDF2PhysXInternal( convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr); - if (rbLinkPtr) + if (rbLinkPtr && mass) { physx::PxRigidBodyExt::updateMassAndInertia(*rbLinkPtr, mass); } diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp index ee30db3a4..f6eb2ae05 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp @@ -1276,6 +1276,8 @@ void EGLRendererVisualShapeConverter::render(const float viewMat[16], const floa render(); + m_data->m_camera.disableVRCamera(); + //cout<m_instancingRenderer && m_data->m_instancingRenderer->getActiveCamera()) + { + *width = m_data->m_window->getWidth() * m_data->m_window->getRetinaScale(); + *height = m_data->m_window->getHeight() * m_data->m_window->getRetinaScale(); + m_data->m_instancingRenderer->getActiveCamera()->getCameraViewMatrix(viewMatrix); + m_data->m_instancingRenderer->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); + m_data->m_instancingRenderer->getActiveCamera()->getCameraUpVector(camUp); + m_data->m_instancingRenderer->getActiveCamera()->getCameraForwardVector(camForward); + + float top = 1.f; + float bottom = -1.f; + float tanFov = (top - bottom) * 0.5f / 1; + float fov = btScalar(2.0) * btAtan(tanFov); + btVector3 camPos, camTarget; + m_data->m_instancingRenderer->getActiveCamera()->getCameraPosition(camPos); + m_data->m_instancingRenderer->getActiveCamera()->getCameraTargetPosition(camTarget); + btVector3 rayFrom = camPos; + btVector3 rayForward = (camTarget - camPos); + rayForward.normalize(); + float farPlane = 10000.f; + rayForward *= farPlane; + + btVector3 rightOffset; + btVector3 cameraUp = btVector3(camUp[0], camUp[1], camUp[2]); + btVector3 vertical = cameraUp; + btVector3 hori; + hori = rayForward.cross(vertical); + hori.normalize(); + vertical = hori.cross(rayForward); + vertical.normalize(); + float tanfov = tanf(0.5f * fov); + hori *= 2.f * farPlane * tanfov; + vertical *= 2.f * farPlane * tanfov; + btScalar aspect = float(*width) / float(*height); + hori *= aspect; + //compute 'hor' and 'vert' vectors, useful to generate raytracer rays + hor[0] = hori[0] * m_data->m_window->getRetinaScale(); + hor[1] = hori[1] * m_data->m_window->getRetinaScale(); + hor[2] = hori[2] * m_data->m_window->getRetinaScale(); + vert[0] = vertical[0] * m_data->m_window->getRetinaScale(); + vert[1] = vertical[1] * m_data->m_window->getRetinaScale(); + vert[2] = vertical[2] * m_data->m_window->getRetinaScale(); + + *yaw = m_data->m_instancingRenderer->getActiveCamera()->getCameraYaw(); + *pitch = m_data->m_instancingRenderer->getActiveCamera()->getCameraPitch(); + *camDist = m_data->m_instancingRenderer->getActiveCamera()->getCameraDistance(); + cameraTarget[0] = camTarget[0]; + cameraTarget[1] = camTarget[1]; + cameraTarget[2] = camTarget[2]; + return true; + } + return false; +} diff --git a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h index 5fe8a5f08..b0cfc36a7 100644 --- a/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h +++ b/examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h @@ -59,6 +59,9 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface virtual void mouseMoveCallback(float x, float y); virtual void mouseButtonCallback(int button, int state, float x, float y); + virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const; + + }; #endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H diff --git a/examples/pybullet/examples/testrender.py b/examples/pybullet/examples/testrender.py index 8be4be7ff..fbdde2aaf 100644 --- a/examples/pybullet/examples/testrender.py +++ b/examples/pybullet/examples/testrender.py @@ -15,7 +15,13 @@ image = plt.imshow(img,interpolation='none',animated=True,label="blah") ax = plt.gca() -pybullet.connect(pybullet.DIRECT) +usePhysX = True +useMaximalCoordinates = True +if usePhysX: + pybullet.connect(pybullet.PhysX,options="--numCores=8 --solver=pgs") + pybullet.loadPlugin("eglRendererPlugin") +else: + pybullet.connect(pybullet.GUI) #pybullet.loadPlugin("eglRendererPlugin") pybullet.loadURDF("plane.urdf",[0,0,-1])