Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Implement PyBullet.getCameraImage for PhysX backend.

Browse Source 
PhysX backend, allow arbitrary plane normal, a few other fixes.
This commit is contained in:
erwincoumans
2019-02-24 14:09:42 -08:00
parent 9ecc1cc485
commit a9996088c8
7 changed files with 481 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
examples/Importers/ImportURDFDemo/UrdfRenderingInterface.h
View File

@@ -95,6 +95,13 @@ struct UrdfRenderingInterface
virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]) {} virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16]) {}
virtual void setProjectiveTexture(bool useProjectiveTexture) {} 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 #endif //LINK_VISUAL_SHAPES_CONVERTER_H

482
examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp
View File

@@ -109,8 +109,9 @@ struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEvent
btSpinMutex m_taskLock; btSpinMutex m_taskLock;
btAlignedObjectArray<b3ContactPointData> m_contactPoints; btAlignedObjectArray<b3ContactPointData> m_contactPoints;
void onContactModify(physx::PxContactModifyPair* const pairs, physx::PxU32 count) void onContactModify(physx::PxContactModifyPair* const pairs, physx::PxU32 count)
{ {
for (physx::PxU32 i = 0; i<count; i++) for (physx::PxU32 i = 0; i<count; i++)
@@ -236,6 +237,9 @@ struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEvent
int m_stateLoggersUniqueId; int m_stateLoggersUniqueId;
std::string m_profileTimingFileName; std::string m_profileTimingFileName;
b3CommandLineArgs m_commandLineArgs; b3CommandLineArgs m_commandLineArgs;
int m_userDebugParametersUid;
btHashMap<btHashInt, double> m_userDebugParameters;
btAlignedObjectArray<int> m_graphicsIndexToSegmentationMask;
PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk, int argc, char* argv[]) PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk, int argc, char* argv[])
: m_isConnected(false), : m_isConnected(false),
@@ -245,7 +249,8 @@ struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEvent
m_pluginManager(sdk), m_pluginManager(sdk),
m_profileTimingLoggingUid(-1), m_profileTimingLoggingUid(-1),
m_stateLoggersUniqueId(1), m_stateLoggersUniqueId(1),
m_commandLineArgs(argc,argv) m_commandLineArgs(argc,argv),
m_userDebugParametersUid(0)
{ {
m_foundation = NULL; 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 //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 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++) for (int link = 1; link < numLinks2; link++)
{ {
int dofs = physxLinks[link]->getInboundJointDof(); int dofs = physxLinks[link]->getInboundJointDof();
physx::PxReal stiffness = 10.f; physx::PxReal stiffness = 10.f;
physx::PxReal damping = 0.1f; physx::PxReal damping = 0.1f;
physx::PxReal forceLimit = PX_MAX_F32; 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) 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) if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
{ {
desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; 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) if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
{ {
kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex]; kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
stiffness = kp; }
} 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; joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity);
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
{
forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex];
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; dofIndex += dofs;
@@ -1262,6 +1269,46 @@ bool PhysXServerCommandProcessor::processSetAdditionalSearchPathCommand(const st
return hasStatus; 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) 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); hasStatus = processSetAdditionalSearchPathCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; 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 #if 0
case CMD_SET_VR_CAMERA_STATE: case CMD_SET_VR_CAMERA_STATE:
{ {
@@ -1415,11 +1475,6 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
break; break;
} }
case CMD_REQUEST_CAMERA_IMAGE_DATA:
{
hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_BODY_INFO: case CMD_REQUEST_BODY_INFO:
{ {
@@ -1553,45 +1608,45 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
break; break;
} }
case CMD_REMOVE_BODY: case CMD_REMOVE_BODY:
{ {
hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processRemoveBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_USER_CONSTRAINT: case CMD_USER_CONSTRAINT:
{ {
hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processCreateUserConstraintCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_CALCULATE_INVERSE_KINEMATICS: case CMD_CALCULATE_INVERSE_KINEMATICS:
{ {
hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processCalculateInverseKinematicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_REQUEST_VISUAL_SHAPE_INFO: case CMD_REQUEST_VISUAL_SHAPE_INFO:
{ {
hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processRequestVisualShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_REQUEST_COLLISION_SHAPE_INFO: case CMD_REQUEST_COLLISION_SHAPE_INFO:
{ {
hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_UPDATE_VISUAL_SHAPE: case CMD_UPDATE_VISUAL_SHAPE:
{ {
hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processUpdateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_CHANGE_TEXTURE: case CMD_CHANGE_TEXTURE:
{ {
hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processChangeTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_LOAD_TEXTURE: case CMD_LOAD_TEXTURE:
{ {
hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processLoadTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_RESTORE_STATE: case CMD_RESTORE_STATE:
{ {
hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
@@ -1605,56 +1660,291 @@ bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryComman
} }
case CMD_LOAD_BULLET: case CMD_LOAD_BULLET:
{ {
hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processLoadBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_SAVE_BULLET: case CMD_SAVE_BULLET:
{ {
hasStatus = processSaveBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processSaveBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_LOAD_MJCF: case CMD_LOAD_MJCF:
{ {
hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_USER_DEBUG_DRAW:
{
hasStatus = processUserDebugDrawCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
break;
}
case CMD_REQUEST_USER_DATA: case CMD_REQUEST_USER_DATA:
{ {
hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processRequestUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_ADD_USER_DATA: case CMD_ADD_USER_DATA:
{ {
hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processAddUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
case CMD_REMOVE_USER_DATA: case CMD_REMOVE_USER_DATA:
{ {
hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); hasStatus = processRemoveUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
break; break;
} }
#endif #endif
default: default:
{ {
BT_PROFILE("CMD_UNKNOWN"); BT_PROFILE("CMD_UNKNOWN");
printf("Unknown command encountered: %d", clientCmd.m_type); printf("Unknown command encountered: %d", clientCmd.m_type);
SharedMemoryStatus& serverCmd = serverStatusOut; SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
hasStatus = true; hasStatus = true;
} }
}; };
return hasStatus; 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 PhysXServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{ {
bool hasStatus = true; bool hasStatus = true;

3
examples/SharedMemory/physx/PhysXServerCommandProcessor.h
View File

@@ -23,11 +23,14 @@ class PhysXServerCommandProcessor : public PhysicsCommandProcessorInterface
bool processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); 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 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 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); bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
void resetSimulation(); void resetSimulation();
bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
public: public:
PhysXServerCommandProcessor(int argc, char* argv[]); PhysXServerCommandProcessor(int argc, char* argv[]);

24
examples/SharedMemory/physx/URDF2PhysX.cpp
View File

@@ -281,11 +281,21 @@ int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedDat
{ {
btVector3 planeNormal = col.m_geometry.m_planeNormal; btVector3 planeNormal = col.m_geometry.m_planeNormal;
btScalar planeConstant = 0; //not available? 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); shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxPlaneGeometry(), *material);
//todo: compensate for plane axis
//physx::PxTransform localPose = tr*physx::PxTransform btTransform diffTr;
//shape->setLocalPose(localPose); 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++; numShapes++;
break; break;
} }
@@ -698,8 +708,8 @@ btTransform ConvertURDF2PhysXInternal(
//Now create the slider and fixed joints... //Now create the slider and fixed joints...
cache.m_articulation->setSolverIterationCounts(4);//todo: API? //cache.m_articulation->setSolverIterationCounts(4);//todo: API?
//cache.m_articulation->setSolverIterationCounts(32);//todo: API? cache.m_articulation->setSolverIterationCounts(32);//todo: API?
cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED); cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED);
cache.m_parentLocalPoses.push_back(physx::PxTransform()); 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); convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr);
if (rbLinkPtr) if (rbLinkPtr && mass)
{ {
physx::PxRigidBodyExt::updateMassAndInertia(*rbLinkPtr, mass); physx::PxRigidBodyExt::updateMassAndInertia(*rbLinkPtr, mass);
} }

58
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
View File

@@ -1276,6 +1276,8 @@ void EGLRendererVisualShapeConverter::render(const float viewMat[16], const floa
render(); render();
m_data->m_camera.disableVRCamera();
//cout<<viewMat[4*0 + 0]<<" "<<viewMat[4*0+1]<<" "<<viewMat[4*0+2]<<" "<<viewMat[4*0+3] << endl; //cout<<viewMat[4*0 + 0]<<" "<<viewMat[4*0+1]<<" "<<viewMat[4*0+2]<<" "<<viewMat[4*0+3] << endl;
//cout<<viewMat[4*1 + 0]<<" "<<viewMat[4*1+1]<<" "<<viewMat[4*1+2]<<" "<<viewMat[4*1+3] << endl; //cout<<viewMat[4*1 + 0]<<" "<<viewMat[4*1+1]<<" "<<viewMat[4*1+2]<<" "<<viewMat[4*1+3] << endl;
//cout<<viewMat[4*2 + 0]<<" "<<viewMat[4*2+1]<<" "<<viewMat[4*2+2]<<" "<<viewMat[4*2+3] << endl; //cout<<viewMat[4*2 + 0]<<" "<<viewMat[4*2+1]<<" "<<viewMat[4*2+2]<<" "<<viewMat[4*2+3] << endl;
@@ -1679,3 +1681,59 @@ void EGLRendererVisualShapeConverter::syncTransform(int collisionObjectUniqueId,
} }
} }
} }
bool EGLRendererVisualShapeConverter::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
{
if (m_data->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;
}

3
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h
View File

@@ -59,6 +59,9 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void mouseMoveCallback(float x, float y); virtual void mouseMoveCallback(float x, float y);
virtual void mouseButtonCallback(int button, int state, 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 #endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H

8
examples/pybullet/examples/testrender.py
View File

@@ -15,7 +15,13 @@ image = plt.imshow(img,interpolation='none',animated=True,label="blah")
ax = plt.gca() 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.loadPlugin("eglRendererPlugin")
pybullet.loadURDF("plane.urdf",[0,0,-1]) pybullet.loadURDF("plane.urdf",[0,0,-1])