diff --git a/data/mjcf/capsule.xml b/data/mjcf/capsule.xml index 7e58ce12c..14fc484d5 100644 --- a/data/mjcf/capsule.xml +++ b/data/mjcf/capsule.xml @@ -5,7 +5,7 @@ - + diff --git a/examples/SharedMemory/PhysicsClientTCP.cpp b/examples/SharedMemory/PhysicsClientTCP.cpp index f959325c0..8ac5b74f8 100644 --- a/examples/SharedMemory/PhysicsClientTCP.cpp +++ b/examples/SharedMemory/PhysicsClientTCP.cpp @@ -181,8 +181,17 @@ bool TcpNetworkedPhysicsProcessor::processCommand(const struct SharedMemoryComma } else { - sz = sizeof(SharedMemoryCommand); - data = (unsigned char*)&clientCmd; + + if (clientCmd.m_type == CMD_REQUEST_VR_EVENTS_DATA) + { + sz = 3 * sizeof(int) + sizeof(smUint64_t) + 16; + data = (unsigned char*)&clientCmd; + } + else + { + sz = sizeof(SharedMemoryCommand); + data = (unsigned char*)&clientCmd; + } } m_data->m_tcpSocket.Send((const uint8 *)data,sz); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3c515834..de467657d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2719,7 +2719,8 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, SaveWorldObjectData sd; sd.m_fileName = fileName; - + int currentOpenGLTextureIndex = 0; + for (int m =0; mm_pluginManager.getRenderInterface()) + { + int totalNumVisualShapes = m_data->m_pluginManager.getRenderInterface()->getNumVisualShapes(bodyUniqueId); + //int totalBytesPerVisualShape = sizeof (b3VisualShapeData); + //int visualShapeStorage = bufferSizeInBytes / totalBytesPerVisualShape - 1; + b3VisualShapeData tmpShape; + + int remain = totalNumVisualShapes - startShapeIndex; + int shapeIndex = startShapeIndex; + + int success = m_data->m_pluginManager.getRenderInterface()->getVisualShapesData(bodyUniqueId,shapeIndex,&tmpShape); + if (success) + { + if (tmpShape.m_tinyRendererTextureId>=0) + { + int openglTextureUniqueId = -1; + + //find companion opengl texture unique id and create a 'textureUid' + if (currentOpenGLTextureIndexm_textureHandles.allocHandle(); + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); + if(texH) + { + texH->m_tinyRendererTextureId = tmpShape.m_tinyRendererTextureId; + texH->m_openglTextureId = openglTextureUniqueId; + } + } + } + } + } } @@ -2893,8 +2932,16 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, int texId = u2b.getAllocatedTexture(i); m_data->m_allocatedTextures.push_back(texId); } - + +/* + int texHandle = m_data->m_textureHandles.allocHandle(); + InternalTextureHandle* texH = m_data->m_textureHandles.getHandle(texHandle); + if(texH) + { + texH->m_tinyRendererTextureId = -1; + texH->m_openglTextureId = -1; + */ for (int i=0;im_tinyRendererTextureId>=0) + { + b3AlignedObjectArray usedHandles; + m_data->m_textureHandles.getUsedHandles(usedHandles); + + for (int i=0;im_textureHandles.getHandle(texHandle); + if (texH && (texH->m_tinyRendererTextureId == visualShapeStoragePtr->m_tinyRendererTextureId)) + { + visualShapeStoragePtr->m_openglTextureId =texH->m_openglTextureId; + visualShapeStoragePtr->m_textureUniqueId = texHandle; + } + } + } + serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1; serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1; serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7e7338e36..b96b0fbfe 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -595,6 +595,11 @@ typedef union { #define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE #define VISUAL_SHAPE_MAX_PATH_LEN 1024 +enum b3VisualShapeDataFlags +{ + eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS = 1, +}; + struct b3VisualShapeData { int m_objectUniqueId; @@ -605,6 +610,10 @@ struct b3VisualShapeData double m_localVisualFrame[7];//pos[3], orn[4] //todo: add more data if necessary (material color etc, although material can be in asset file .obj file) double m_rgbaColor[4]; + int m_tinyRendererTextureId; + int m_textureUniqueId; + int m_openglTextureId; + }; struct b3VisualShapeInformation @@ -743,6 +752,8 @@ enum eCONNECT_METHOD { eCONNECT_GUI_SERVER=7, eCONNECT_GUI_MAIN_THREAD=8, eCONNECT_SHARED_MEMORY_SERVER=9, + eCONNECT_DART=10, + eCONNECT_MUJOCO=11, }; enum eURDF_Flags diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index 496faea40..cf148fce5 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -155,6 +155,69 @@ btVector3 b3RobotSimulatorClientAPI_NoDirect::getEulerFromQuaternion(const btQua return rpy2; } +int b3RobotSimulatorClientAPI_NoDirect::loadTexture(const std::string& fileName) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return -1; + } + btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + { + commandHandle = b3InitLoadTexture(m_data->m_physicsClientHandle, fileName.c_str()); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_LOAD_TEXTURE_COMPLETED) + { + return b3GetStatusTextureUniqueId(statusHandle); + } + } + return -1; +} + + +bool b3RobotSimulatorClientAPI_NoDirect::changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args) +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + int objectUniqueId = args.m_objectUniqueId; + int jointIndex = args.m_linkIndex; + int shapeIndex = args.m_shapeIndex; + int textureUniqueId = args.m_textureUniqueId; + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + commandHandle = b3InitUpdateVisualShape(m_data->m_physicsClientHandle, objectUniqueId, jointIndex, shapeIndex, textureUniqueId); + + if (args.m_hasSpecularColor) + { + double specularColor[3] = {args.m_specularColor[0],args.m_specularColor[1],args.m_specularColor[2]}; + b3UpdateVisualShapeSpecularColor(commandHandle,specularColor); + } + if (args.m_hasRgbaColor) + { + double rgbaColor[4] = {args.m_rgbaColor[0],args.m_rgbaColor[1],args.m_rgbaColor[2],args.m_rgbaColor[3]}; + b3UpdateVisualShapeRGBAColor(commandHandle,rgbaColor); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + statusType = b3GetStatusType(statusHandle); + + return (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED); +} + int b3RobotSimulatorClientAPI_NoDirect::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) { int robotUniqueId = -1; @@ -2177,7 +2240,8 @@ void b3RobotSimulatorClientAPI_NoDirect::restoreStateFromMemory(int stateId) bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; - if (sm == 0) { + if (sm == 0) + { b3Warning("Not connected"); return false; } @@ -2185,17 +2249,17 @@ bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3 b3SharedMemoryStatusHandle statusHandle; int statusType; - { - commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId); - statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); - statusType = b3GetStatusType(statusHandle); + commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); - btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED); - if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) { - b3GetVisualShapeInformation(sm, &visualShapeInfo); - } - return true; - } + btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED); + if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) + { + b3GetVisualShapeInformation(sm, &visualShapeInfo); + return true; + } + return false; } void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(const std::string& path) diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 5c8a6a424..a58b40d26 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -60,6 +60,30 @@ struct b3RobotSimulatorLoadFileResults } }; +struct b3RobotSimulatorChangeVisualShapeArgs +{ + int m_objectUniqueId; + int m_linkIndex; + int m_shapeIndex; + int m_textureUniqueId; + btVector4 m_rgbaColor; + bool m_hasRgbaColor; + btVector3 m_specularColor; + bool m_hasSpecularColor; + + b3RobotSimulatorChangeVisualShapeArgs() + :m_objectUniqueId(-1), + m_linkIndex(-1), + m_shapeIndex(-1), + m_textureUniqueId(-1), + m_rgbaColor(0,0,0,1), + m_hasRgbaColor(false), + m_specularColor(1,1,1), + m_hasSpecularColor(false) + { + } +}; + struct b3RobotSimulatorJointMotorArgs { int m_controlMode; @@ -505,6 +529,10 @@ public: bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool saveBullet(const std::string& fileName); + int loadTexture(const std::string& fileName); + + bool changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args); + bool savePythonWorld(const std::string& fileName); bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo); @@ -648,7 +676,7 @@ public: bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo); - bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo); + bool getVisualShapeData(int bodyUniqueId, struct b3VisualShapeInformation &visualShapeInfo); int saveStateToMemory(); void restoreStateFromMemory(int stateId); diff --git a/examples/SharedMemory/dart/DARTPhysicsC_API.cpp b/examples/SharedMemory/dart/DARTPhysicsC_API.cpp new file mode 100644 index 000000000..785f17f61 --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsC_API.cpp @@ -0,0 +1,16 @@ +#ifdef BT_ENABLE_DART +#include "DARTPhysicsC_API.h" +#include "DARTPhysicsServerCommandProcessor.h" +#include "DARTPhysicsClient.h" + +//think more about naming. The b3ConnectPhysicsLoopback +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART() +{ + DARTPhysicsServerCommandProcessor* sdk = new DARTPhysicsServerCommandProcessor; + + DARTPhysicsClient* direct = new DARTPhysicsClient(sdk,true); + bool connected; + connected = direct->connect(); + return (b3PhysicsClientHandle )direct; +} +#endif//BT_ENABLE_DART \ No newline at end of file diff --git a/examples/SharedMemory/dart/DARTPhysicsC_API.h b/examples/SharedMemory/dart/DARTPhysicsC_API.h new file mode 100644 index 000000000..e268c8dcf --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsC_API.h @@ -0,0 +1,20 @@ +#ifndef DART_PHYSICS_C_API_H +#define DART_PHYSICS_C_API_H + +#ifdef BT_ENABLE_DART + +#include "../PhysicsClientC_API.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//think more about naming. The b3ConnectPhysicsLoopback +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDART(); + +#ifdef __cplusplus +} +#endif + +#endif //BT_ENABLE_DART +#endif //DART_PHYSICS_C_API_H diff --git a/examples/SharedMemory/dart/DARTPhysicsClient.cpp b/examples/SharedMemory/dart/DARTPhysicsClient.cpp new file mode 100644 index 000000000..19e99202d --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsClient.cpp @@ -0,0 +1,1596 @@ +#include "DARTPhysicsClient.h" + +#include "../PhysicsClientSharedMemory.h" +#include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemoryCommands.h" +#include "../PhysicsCommandProcessorInterface.h" +#include "../../Utils/b3Clock.h" + +#include "LinearMath/btHashMap.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "../../../Extras/Serialize/BulletFileLoader/btBulletFile.h" +#include "../../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" +#include "../BodyJointInfoUtility.h" +#include + +#include "../SharedMemoryUserData.h" +#include "LinearMath/btQuickprof.h" + +struct DARTUserDataCache { + btHashMap m_userDataMap; + btHashMap m_keyToUserDataIdMap; + + ~DARTUserDataCache() + { + + } +}; + +struct BodyJointInfoCache2 +{ + std::string m_baseName; + btAlignedObjectArray m_jointInfo; + std::string m_bodyName; + + // Joint index -> user data. + btHashMap m_jointToUserDataMap; + + ~BodyJointInfoCache2() { + } +}; + + + +struct DARTPhysicsDirectInternalData +{ + DummyGUIHelper m_noGfx; + + btAlignedObjectArray m_serverDNA; + SharedMemoryCommand m_command; + SharedMemoryStatus m_serverStatus; + + SharedMemoryCommand m_tmpInfoRequestCommand; + SharedMemoryStatus m_tmpInfoStatus; + bool m_hasStatus; + bool m_verboseOutput; + + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; + + btHashMap m_bodyJointMap; + btHashMap m_userConstraintInfoMap; + + btAlignedObjectArray m_profileTimings; + btHashMap m_profileTimingStringArray; + + char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + btAlignedObjectArray m_cachedMassMatrix; + int m_cachedCameraPixelsWidth; + int m_cachedCameraPixelsHeight; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_cachedSegmentationMask; + + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArray m_cachedOverlappingObjects; + + btAlignedObjectArray m_cachedVisualShapes; + btAlignedObjectArray m_cachedCollisionShapes; + + btAlignedObjectArray m_cachedVREvents; + + btAlignedObjectArray m_cachedKeyboardEvents; + btAlignedObjectArray m_cachedMouseEvents; + + btAlignedObjectArray m_raycastHits; + + PhysicsCommandProcessorInterface* m_commandProcessor; + bool m_ownsCommandProcessor; + double m_timeOutInSeconds; + + DARTPhysicsDirectInternalData() + :m_hasStatus(false), + m_verboseOutput(false), + m_cachedCameraPixelsWidth(0), + m_cachedCameraPixelsHeight(0), + m_commandProcessor(NULL), + m_ownsCommandProcessor(false), + m_timeOutInSeconds(1e30) + { + memset(&m_command, 0, sizeof(m_command)); + memset(&m_serverStatus, 0, sizeof(m_serverStatus)); + memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient)); + } +}; + +DARTPhysicsClient::DARTPhysicsClient(PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership) +{ + int sz = sizeof(SharedMemoryCommand); + int sz2 = sizeof(SharedMemoryStatus); + + m_data = new DARTPhysicsDirectInternalData; + m_data->m_commandProcessor = physSdk; + m_data->m_ownsCommandProcessor = passSdkOwnership; + +} + +DARTPhysicsClient::~DARTPhysicsClient() +{ + for (int i=0;im_profileTimingStringArray.size();i++) + { + std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i); + if (str) + { + delete *str; + } + } + m_data->m_profileTimingStringArray.clear(); + + if (m_data->m_commandProcessor->isConnected()) + { + m_data->m_commandProcessor->disconnect(); + } + if (m_data->m_ownsCommandProcessor) + { + delete m_data->m_commandProcessor; + } + + resetData(); + + + + delete m_data; +} + +void DARTPhysicsClient::resetData() +{ + m_data->m_debugLinesFrom.clear(); + m_data->m_debugLinesTo.clear(); + m_data->m_debugLinesColor.clear(); + for (int i = 0; im_bodyJointMap.size(); i++) + { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); + if (bodyJointsPtr && *bodyJointsPtr) + { + delete (*bodyJointsPtr); + } + } + m_data->m_bodyJointMap.clear(); + m_data->m_userConstraintInfoMap.clear(); +} + +// return true if connection succesfull, can also check 'isConnected' +bool DARTPhysicsClient::connect() +{ + bool connected = m_data->m_commandProcessor->connect(); + m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + + + if (connected) + //also request serialization data + { + SharedMemoryCommand command; + command.m_type = CMD_REQUEST_INTERNAL_DATA; + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + postProcessStatus(m_data->m_serverStatus); + } + else + { + b3Clock clock; + double timeSec = clock.getTimeInSeconds(); + + while ((!hasStatus) && (clock.getTimeInSeconds()-timeSec <10 )) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + } + } + + return connected; +} + +// return true if connection succesfull, can also check 'isConnected' +bool DARTPhysicsClient::connect(struct GUIHelperInterface* guiHelper) +{ + bool connected = m_data->m_commandProcessor->connect(); + + m_data->m_commandProcessor->setGuiHelper(guiHelper); + + return connected; +} + +void DARTPhysicsClient::renderScene() +{ + int renderFlags = 0; + m_data->m_commandProcessor->renderScene(renderFlags); +} + +void DARTPhysicsClient::debugDraw(int debugDrawMode) +{ + m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode); +} + +////todo: rename to 'disconnect' +void DARTPhysicsClient::disconnectSharedMemory() +{ + m_data->m_commandProcessor->disconnect(); + m_data->m_commandProcessor->setGuiHelper(0); +} + +bool DARTPhysicsClient::isConnected() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +// return non-null if there is a status, nullptr otherwise +const SharedMemoryStatus* DARTPhysicsClient::processServerStatus() +{ + + if (!m_data->m_hasStatus) + { + m_data->m_hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + SharedMemoryStatus* stat = 0; + if (m_data->m_hasStatus) + { + stat = &m_data->m_serverStatus; + + postProcessStatus(m_data->m_serverStatus); + + m_data->m_hasStatus = false; + } + return stat; +} + +SharedMemoryCommand* DARTPhysicsClient::getAvailableSharedMemoryCommand() +{ + return &m_data->m_command; +} + +bool DARTPhysicsClient::canSubmitCommand() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +bool DARTPhysicsClient::processDebugLines(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == CMD_DEBUG_LINES_COMPLETED); + + if (m_data->m_verboseOutput) + { + b3Printf("Success receiving %d debug lines", + serverCmd.m_sendDebugLinesArgs.m_numDebugLines); + } + + int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; + float* linesFrom = + (float*)&m_data->m_bulletStreamDataServerToClient[0]; + float* linesTo = + (float*)(&m_data->m_bulletStreamDataServerToClient[0] + + numLines * 3 * sizeof(float)); + float* linesColor = + (float*)(&m_data->m_bulletStreamDataServerToClient[0] + + 2 * numLines * 3 * sizeof(float)); + + m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesColor.resize( + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines); + + for (int i = 0; i < numLines; i++) + { + TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1], + linesFrom[i * 3 + 2]); + TmpFloat3 to = + CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]); + TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1], + linesColor[i * 3 + 2]); + + m_data + ->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + from; + m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + to; + m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + i] = color; + } + + if (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0) + { + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_DEBUG_LINES; + command.m_requestDebugLinesArguments.m_startingLineIndex = + serverCmd.m_sendDebugLinesArgs.m_numDebugLines + + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; + } + } + + } while (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0); + + return m_data->m_hasStatus; +} + +bool DARTPhysicsClient::processVisualShapeData(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + + if (m_data->m_verboseOutput) + { + b3Printf("Visual Shape Information Request OK\n"); + } + int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex; + int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied); + b3VisualShapeData* shapeData = (b3VisualShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numVisualShapesCopied; i++) + { + m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i]; + } + + if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied) + { + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; + command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId; + } + } + } while (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied); + + return m_data->m_hasStatus; +} + +bool DARTPhysicsClient::processOverlappingObjects(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Overlapping Objects Request OK\n"); + } + + int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex; + int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied); + b3OverlappingObject* objects = (b3OverlappingObject*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i = 0; i < numOverlapCopied; i++) + { + m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i]; + } + + if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied) + { + m_data->m_hasStatus = false; + command.m_type = CMD_REQUEST_AABB_OVERLAP; + command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + } + + } + } while (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied); + + return m_data->m_hasStatus; + +} + + + +bool DARTPhysicsClient::processContactPointData(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Contact Point Information Request OK\n"); + } + int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; + int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied); + + b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i=0;im_cachedContactPoints[startContactIndex+i] = contactData[i]; + } + + if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) + { + + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; + command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + command.m_requestContactPointArguments.m_objectAIndexFilter = -1; + command.m_requestContactPointArguments.m_objectBIndexFilter = -1; + + } + + } + } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied); + + return m_data->m_hasStatus; + +} + + +bool DARTPhysicsClient::processCamera(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED); + + if (m_data->m_verboseOutput) + { + b3Printf("Camera image OK\n"); + } + + int numBytesPerPixel = 4;//RGBA + int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+ + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+ + serverCmd.m_sendPixelDataArguments.m_numRemainingPixels; + + m_data->m_cachedCameraPixelsWidth = 0; + m_data->m_cachedCameraPixelsHeight = 0; + + int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight; + + m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel); + m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); + m_data->m_cachedSegmentationMask.resize(numTotalPixels); + m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel); + + + unsigned char* rgbaPixelsReceived = + (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; + + float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]); + int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]); + + // printf("pixel = %d\n", rgbaPixelsReceived[0]); + + for (int i=0;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } + for (int i=0;im_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i]; + } + for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] + = rgbaPixelsReceived[i]; + } + + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) + { + + m_data->m_hasStatus = false; + + // continue requesting remaining pixels + command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA; + command.m_requestPixelDataArguments.m_startPixelIndex = + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; + + } else + { + m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth; + m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; + } + } + } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied); + + return m_data->m_hasStatus; + + +} + + +void DARTPhysicsClient::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + + BodyJointInfoCache2** cachePtr = m_data->m_bodyJointMap[bodyUniqueId]; + //don't process same bodyUniqueId multiple times + if (cachePtr) + { + return; + } + + bParse::btBulletFile bf( + &m_data->m_bulletStreamDataServerToClient[0], + serverCmd.m_numDataStreamBytes); + if (m_data->m_serverDNA.size()) + { + bf.setFileDNA(false, &m_data->m_serverDNA[0], m_data->m_serverDNA.size()); + } + else + { + bf.setFileDNAisMemoryDNA(); + } + bf.parse(false); + + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; + + for (int i = 0; i < bf.m_multiBodies.size(); i++) + { + int flag = bf.getFlags(); + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) + { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } + } + if (bf.ok()) + { + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } + } else + { + b3Warning("Robot description not received"); + } +} + +void DARTPhysicsClient::processAddUserData(const struct SharedMemoryStatus& serverCmd) { + const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + if (!userDataCachePtr) + { + DARTUserDataCache cache; + (*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache); + } + userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + + const char *dataStream = m_data->m_bulletStreamDataServerToClient; + + b3UserDataValue userDataValue; + userDataValue.m_type = serverCmd.m_userDataResponseArgs.m_valueType; + userDataValue.m_length = serverCmd.m_userDataResponseArgs.m_valueLength; + SharedMemoryUserData *userDataPtr = userDataCachePtr->m_userDataMap[userDataGlobalId.m_userDataId]; + if (userDataPtr) { + // Only replace the value. + (userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + } + else { + // Add a new user data entry. + (userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key)); + userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; + userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + (userDataCachePtr)->m_keyToUserDataIdMap.insert(serverCmd.m_userDataResponseArgs.m_key, userDataGlobalId.m_userDataId); + } + } +} + +void DARTPhysicsClient::postProcessStatus(const struct SharedMemoryStatus& serverCmd) +{ + switch (serverCmd.m_type) + { + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Raycast completed"); + } + m_data->m_raycastHits.clear(); + b3RayHitInfo *rayHits = (b3RayHitInfo *)m_data->m_bulletStreamDataServerToClient; + for (int i=0;im_raycastHits.push_back(rayHits[i]); + } + break; + } + case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED: + { + + if (m_data->m_verboseOutput) + { + b3Printf("Request VR Events completed"); + } + m_data->m_cachedVREvents.resize(serverCmd.m_sendVREvents.m_numVRControllerEvents); + for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++) + { + m_data->m_cachedVREvents[i] = serverCmd.m_sendVREvents.m_controllerEvents[i]; + } + break; + } + case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Request keyboard events completed"); + } + m_data->m_cachedKeyboardEvents.resize(serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents); + for (int i=0;im_cachedKeyboardEvents[i] = serverCmd.m_sendKeyboardEvents.m_keyboardEvents[i]; + } + break; + } + + case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED: + { + B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Request mouse events completed"); + } + m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents); + for (int i=0;im_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i]; + } + break; + } + + case CMD_REQUEST_INTERNAL_DATA_COMPLETED: + { + if (serverCmd.m_numDataStreamBytes) + { + int numStreamBytes = serverCmd.m_numDataStreamBytes; + m_data->m_serverDNA.resize(numStreamBytes); + for (int i = 0; i < numStreamBytes; i++) + { + m_data->m_serverDNA[i] = m_data->m_bulletStreamDataServerToClient[i]; + } + } + break; + } + case CMD_RESET_SIMULATION_COMPLETED: + { + resetData(); + break; + } + + case CMD_USER_CONSTRAINT_INFO_COMPLETED: + case CMD_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.remove(cid); + break; + } + case CMD_REMOVE_BODY_FAILED: + { + b3Warning("Remove body failed\n"); + break; + } + case CMD_REMOVE_BODY_COMPLETED: + { + for (int i=0;im_userConstraintInfoMap.remove(key); + } + + break; + } + case CMD_CHANGE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid]; + if (userConstraintPtr) + { + const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs; + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) + { + userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0]; + userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1]; + userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) + { + userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3]; + userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4]; + userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5]; + userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + userConstraintPtr->m_erp = serverConstraint->m_erp; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink; + } + } + break; + } + case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED: + { + break; + } + case CMD_SYNC_BODY_INFO_COMPLETED: + case CMD_MJCF_LOADING_COMPLETED: + case CMD_SDF_LOADING_COMPLETED: + { + //we'll stream further info from the physics server + //so serverCmd will be invalid, make a copy + + int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints; + for (int i=0;im_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT; + m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; + m_data->m_tmpInfoRequestCommand.m_userConstraintArguments.m_userConstraintUniqueId = constraintUid; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + int cid = m_data->m_tmpInfoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid,m_data->m_tmpInfoStatus.m_userConstraintResultArgs); + } + } + + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + for (int i = 0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO; + m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processBodyJointInfo(bodyUniqueId, m_data->m_tmpInfoStatus); + } + } + break; + } + case CMD_CREATE_MULTI_BODY_COMPLETED: + case CMD_URDF_LOADING_COMPLETED: + { + + if (serverCmd.m_numDataStreamBytes > 0) + { + int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyIndex, serverCmd); + } + break; + } + case CMD_BULLET_LOADING_FAILED: + { + b3Warning("Couldn't load .bullet file"); + break; + } + case CMD_BULLET_LOADING_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: + { + b3Warning("requestOpenGLVisualizeCamera failed"); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_FAILED: + { + b3Warning("removeConstraint failed"); + break; + } + case CMD_CHANGE_USER_CONSTRAINT_FAILED: + { + //b3Warning("changeConstraint failed"); + break; + } + + case CMD_USER_CONSTRAINT_FAILED: + { + b3Warning("createConstraint failed"); + break; + } + + case CMD_CREATE_COLLISION_SHAPE_FAILED: + { + b3Warning("createCollisionShape failed"); + break; + } + case CMD_CREATE_COLLISION_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_VISUAL_SHAPE_FAILED: + { + b3Warning("createVisualShape failed"); + break; + } + case CMD_CREATE_VISUAL_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_MULTI_BODY_FAILED: + { + b3Warning("createMultiBody failed"); + break; + } + case CMD_REQUEST_COLLISION_INFO_COMPLETED: + { + break; + } + case CMD_REQUEST_COLLISION_INFO_FAILED: + { + b3Warning("Request getCollisionInfo failed"); + break; + } + + case CMD_CUSTOM_COMMAND_COMPLETED: + { + break; + } + case CMD_CUSTOM_COMMAND_FAILED: + { + b3Warning("custom plugin command failed"); + break; + } + case CMD_CLIENT_COMMAND_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_FAILED: + { + b3Warning("jacobian calculation failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_FAILED: + { + b3Warning("calculate mass matrix failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_COMPLETED: + { + double* matrixData = (double*)&m_data->m_bulletStreamDataServerToClient[0]; + m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount); + for (int i=0;im_cachedMassMatrix[i] = matrixData[i]; + } + break; + } + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: + { + break; + } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: + { + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: + { + break; + } + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: + { + break; + } + case CMD_SAVE_STATE_COMPLETED: + { + break; + } + case CMD_COLLISION_SHAPE_INFO_FAILED: + { + b3Warning("getCollisionShapeData failed"); + break; + } + case CMD_COLLISION_SHAPE_INFO_COMPLETED: + { + B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Collision Shape Information Request OK\n"); + } + int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes; + m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied); + b3CollisionShapeData* shapeData = (b3CollisionShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numCollisionShapesCopied; i++) + { + m_data->m_cachedCollisionShapes[i] = shapeData[i]; + } + break; + } + case CMD_RESTORE_STATE_FAILED: + { + b3Warning("restoreState failed"); + break; + } + case CMD_RESTORE_STATE_COMPLETED: + { + break; + } + case CMD_BULLET_SAVING_COMPLETED: + { + break; + } + case CMD_LOAD_SOFT_BODY_FAILED: + { + b3Warning("loadSoftBody failed"); + break; + } + case CMD_LOAD_SOFT_BODY_COMPLETED: + { + break; + } + case CMD_SYNC_USER_DATA_FAILED: + { + b3Warning("Synchronizing user data failed."); + break; + } + case CMD_ADD_USER_DATA_FAILED: + { + b3Warning("Adding user data failed (do the specified body and link exist?)"); + break; + } + case CMD_REMOVE_USER_DATA_FAILED: + { + b3Warning("Removing user data failed"); + break; + } + case CMD_ADD_USER_DATA_COMPLETED: + { + processAddUserData(serverCmd); + break; + } + case CMD_SYNC_USER_DATA_COMPLETED: + { + B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); + // Remove all cached user data entries. + for(int i=0; im_bodyJointMap.size(); i++) + { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); + if (bodyJointsPtr && *bodyJointsPtr) + { + (*bodyJointsPtr)->m_jointToUserDataMap.clear(); + } + } + const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; + b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers]; + memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier)); + + for (int i=0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA; + m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = identifiers[i]; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processAddUserData(m_data->m_tmpInfoStatus); + } + } + delete[] identifiers; + break; + } + case CMD_REMOVE_USER_DATA_COMPLETED: + { + const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { + DARTUserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + if (userDataCachePtr) + { + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; + if (userDataPtr) { + (userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str()); + (userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId); + } + } + } + break; + } + default: + { + //b3Warning("Unknown server status type"); + } + }; + + +} +bool DARTPhysicsClient::submitClientCommand(const struct SharedMemoryCommand& command) +{ + if (command.m_type==CMD_REQUEST_DEBUG_LINES) + { + return processDebugLines(command); + } + + if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA) + { + return processCamera(command); + } + if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION) + { + return processContactPointData(command); + } + + if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO) + { + return processVisualShapeData(command); + } + if (command.m_type == CMD_REQUEST_AABB_OVERLAP) + { + return processOverlappingObjects(command); + } + + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + m_data->m_hasStatus = hasStatus; + /*if (hasStatus) + { + postProcessStatus(m_data->m_serverStatus); + m_data->m_hasStatus = false; + } + */ + return hasStatus; +} + +int DARTPhysicsClient::getNumBodies() const +{ + return m_data->m_bodyJointMap.size(); +} + +void DARTPhysicsClient::removeCachedBody(int bodyUniqueId) +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + delete (*bodyJointsPtr); + m_data->m_bodyJointMap.remove(bodyUniqueId); + } +} + + +int DARTPhysicsClient::getNumUserConstraints() const +{ + return m_data->m_userConstraintInfoMap.size(); +} + +int DARTPhysicsClient::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const +{ + b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId]; + if (constraintPtr) + { + info = *constraintPtr; + return 1; + } + return 0; +} + +int DARTPhysicsClient::getUserConstraintId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) + { + return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +int DARTPhysicsClient::getBodyUniqueId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumBodies())) + { + return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +bool DARTPhysicsClient::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + strcpy(info.m_baseName,bodyJoints->m_baseName.c_str()); + strcpy(info.m_bodyName ,bodyJoints->m_bodyName .c_str()); + return true; + } + + return false; +} + +int DARTPhysicsClient::getNumJoints(int bodyIndex) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + return bodyJoints->m_jointInfo.size(); + } + btAssert(0); + return 0; +} + +bool DARTPhysicsClient::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + if ((jointIndex >=0) && (jointIndex < bodyJoints->m_jointInfo.size())) + { + info = bodyJoints->m_jointInfo[jointIndex]; + return true; + } + } + return false; +} + + +void DARTPhysicsClient::setSharedMemoryKey(int key) +{ +} + + +void DARTPhysicsClient::uploadBulletFileToSharedMemory(const char* data, int len) +{ + if (len>SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) + { + len = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; + } + for (int i=0;im_bulletStreamDataServerToClient[i] = data[i]; + } + //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); +} + +void DARTPhysicsClient::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) +{ + int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays; + int newNumRays = curNumStreamingRays + numRays; + btAssert(newNumRaysm_bulletStreamDataServerToClient; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; + command.m_requestRaycastIntersections.m_numStreamingRays++; + } + + } + +} + + +int DARTPhysicsClient::getNumDebugLines() const +{ + return m_data->m_debugLinesFrom.size(); +} + +const float* DARTPhysicsClient::getDebugLinesFrom() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesFrom[0].m_x; + } + return 0; +} +const float* DARTPhysicsClient::getDebugLinesTo() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesTo[0].m_x; + } + return 0; +} +const float* DARTPhysicsClient::getDebugLinesColor() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesColor[0].m_x; + } + return 0; +} + +void DARTPhysicsClient::getCachedCameraImage(b3CameraImageData* cameraData) +{ + if (cameraData) + { + cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; + cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; + cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; + cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0; + } +} + +void DARTPhysicsClient::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); + contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0; + +} + +void DARTPhysicsClient::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) +{ + overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size(); + overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ? + &m_data->m_cachedOverlappingObjects[0] : 0; +} + + +void DARTPhysicsClient::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) +{ + visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); + visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; +} + +void DARTPhysicsClient::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) +{ + collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size(); + collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0; +} + + + +void DARTPhysicsClient::getCachedVREvents(struct b3VREventsData* vrEventsData) +{ + vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size(); + vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents? + &m_data->m_cachedVREvents[0] : 0; +} + +void DARTPhysicsClient::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) +{ + keyboardEventsData->m_numKeyboardEvents = m_data->m_cachedKeyboardEvents.size(); + keyboardEventsData->m_keyboardEvents = keyboardEventsData->m_numKeyboardEvents? + &m_data->m_cachedKeyboardEvents[0] : 0; +} + +void DARTPhysicsClient::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size(); + mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents? + &m_data->m_cachedMouseEvents[0] : 0; +} + + +void DARTPhysicsClient::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) +{ + raycastHits->m_numRayHits = m_data->m_raycastHits.size(); + raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0; +} + +void DARTPhysicsClient::getCachedMassMatrix(int dofCountCheck, double* massMatrix) +{ + int sz = dofCountCheck*dofCountCheck; + if (sz == m_data->m_cachedMassMatrix.size()) + { + for (int i=0;im_cachedMassMatrix[i]; + } + } +} + +void DARTPhysicsClient::setTimeOut(double timeOutInSeconds) +{ + m_data->m_timeOutInSeconds = timeOutInSeconds; +} + +double DARTPhysicsClient::getTimeOut() const +{ + return m_data->m_timeOutInSeconds; +} + +bool DARTPhysicsClient::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return false; + } + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) + { + return false; + } + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId]; + if (!userDataPtr) + { + return false; + } + valueOut.m_type = userDataPtr->m_type; + valueOut.m_length = userDataPtr->m_bytes.size(); + valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0; + return true; +} + +int DARTPhysicsClient::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return -1; + } + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) { + return -1; + } + int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key]; + if (!userDataId) { + return -1; + } + return *userDataId; +} + +int DARTPhysicsClient::getNumUserData(int bodyUniqueId, int linkIndex) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return 0; + } + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) { + return 0; + } + return (userDataCachePtr)->m_userDataMap.size(); +} + +void DARTPhysicsClient::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + *keyOut = 0; + *userDataIdOut = -1; + return; + } + DARTUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size()) + { + *keyOut = 0; + *userDataIdOut = -1; + return; + } + *userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1(); + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex); + *keyOut = (userDataPtr)->m_key.c_str(); +} + + + +void DARTPhysicsClient::pushProfileTiming(const char* timingName) +{ + std::string** strPtr = m_data->m_profileTimingStringArray[timingName]; + std::string* str = 0; + if (strPtr) + { + str = *strPtr; + } else + { + str = new std::string(timingName); + m_data->m_profileTimingStringArray.insert(timingName,str); + } + m_data->m_profileTimings.push_back(new CProfileSample(str->c_str())); +} + + +void DARTPhysicsClient::popProfileTiming() +{ + if (m_data->m_profileTimings.size()) + { + CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size()-1]; + m_data->m_profileTimings.pop_back(); + delete sample; + } +} diff --git a/examples/SharedMemory/dart/DARTPhysicsClient.h b/examples/SharedMemory/dart/DARTPhysicsClient.h new file mode 100644 index 000000000..466aaeb50 --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsClient.h @@ -0,0 +1,124 @@ +#ifndef DART_PHYSICS_CLIENT_H +#define DART_PHYSICS_CLIENT_H + + +#include "../PhysicsClient.h" + +///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands +class DARTPhysicsClient : public PhysicsClient +{ +protected: + + struct DARTPhysicsDirectInternalData* m_data; + + bool processDebugLines(const struct SharedMemoryCommand& orgCommand); + + bool processCamera(const struct SharedMemoryCommand& orgCommand); + + bool processContactPointData(const struct SharedMemoryCommand& orgCommand); + + bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand); + + bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand); + + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + + void processAddUserData(const struct SharedMemoryStatus& serverCmd); + + void postProcessStatus(const struct SharedMemoryStatus& serverCmd); + + void resetData(); + + void removeCachedBody(int bodyUniqueId); + +public: + + DARTPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership); + + virtual ~DARTPhysicsClient(); + + // return true if connection succesfull, can also check 'isConnected' + //it is OK to pass a null pointer for the gui helper + virtual bool connect(); + + ////todo: rename to 'disconnect' + virtual void disconnectSharedMemory(); + + virtual bool isConnected() const; + + // return non-null if there is a status, nullptr otherwise + virtual const SharedMemoryStatus* processServerStatus(); + + virtual SharedMemoryCommand* getAvailableSharedMemoryCommand(); + + virtual bool canSubmitCommand() const; + + virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + + virtual int getNumBodies() const; + + virtual int getBodyUniqueId(int serialIndex) const; + + virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const; + + virtual int getNumJoints(int bodyIndex) const; + + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + + virtual int getNumUserConstraints() const; + + virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; + + virtual int getUserConstraintId(int serialIndex) const; + + ///todo: move this out of the + virtual void setSharedMemoryKey(int key); + + void uploadBulletFileToSharedMemory(const char* data, int len); + + virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays); + + virtual int getNumDebugLines() const; + + virtual const float* getDebugLinesFrom() const; + virtual const float* getDebugLinesTo() const; + virtual const float* getDebugLinesColor() const; + + virtual void getCachedCameraImage(b3CameraImageData* cameraData); + + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + + virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); + + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); + + virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo); + + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); + + virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); + + virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData); + + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + + virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + + //the following APIs are for internal use for visualization: + virtual bool connect(struct GUIHelperInterface* guiHelper); + virtual void renderScene(); + virtual void debugDraw(int debugDrawMode); + + virtual void setTimeOut(double timeOutInSeconds); + virtual double getTimeOut() const; + + virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; + virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + + virtual void pushProfileTiming(const char* timingName); + virtual void popProfileTiming(); +}; + +#endif //DART_PHYSICS__H diff --git a/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.cpp b/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.cpp new file mode 100644 index 000000000..713f7d654 --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.cpp @@ -0,0 +1,39 @@ +#include "DARTPhysicsServerCommandProcessor.h" + + +DARTPhysicsServerCommandProcessor::DARTPhysicsServerCommandProcessor() +{ +} + +DARTPhysicsServerCommandProcessor::~DARTPhysicsServerCommandProcessor() +{ + +} + +bool DARTPhysicsServerCommandProcessor::connect() +{ + return false; +} + +void DARTPhysicsServerCommandProcessor::disconnect() +{ + +} + +bool DARTPhysicsServerCommandProcessor::isConnected() const +{ + return false; +} + +bool DARTPhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + return false; +} + +bool DARTPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + return false; +} + + + diff --git a/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.h b/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.h new file mode 100644 index 000000000..bbbfbfa19 --- /dev/null +++ b/examples/SharedMemory/dart/DARTPhysicsServerCommandProcessor.h @@ -0,0 +1,31 @@ +#ifndef DART_PHYSICS_SERVER_COMMAND_PROCESSOR_H +#define DART_PHYSICS_SERVER_COMMAND_PROCESSOR_H + +#include "../PhysicsCommandProcessorInterface.h" + +class DARTPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface +{ + +public: + DARTPhysicsServerCommandProcessor(); + + virtual ~DARTPhysicsServerCommandProcessor(); + + virtual bool connect(); + + virtual void disconnect(); + + virtual bool isConnected() const; + + virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual void renderScene(int renderFlags){} + virtual void physicsDebugDraw(int debugDrawFlags){} + virtual void setGuiHelper(struct GUIHelperInterface* guiHelper){} + virtual void setTimeOut(double timeOutInSeconds){} + +}; + +#endif //DART_PHYSICS_COMMAND_PROCESSOR_H diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp b/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp new file mode 100644 index 000000000..2372a879f --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.cpp @@ -0,0 +1,15 @@ +#ifdef BT_ENABLE_MUJOCO +#include "MuJoCoPhysicsC_API.h" +#include "MuJoCoPhysicsServerCommandProcessor.h" +#include "MuJoCoPhysicsClient.h" + +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo() +{ + MuJoCoPhysicsServerCommandProcessor* sdk = new MuJoCoPhysicsServerCommandProcessor; + + MuJoCoPhysicsClient* direct = new MuJoCoPhysicsClient(sdk,true); + bool connected; + connected = direct->connect(); + return (b3PhysicsClientHandle )direct; +} +#endif//BT_ENABLE_MUJOCO \ No newline at end of file diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h b/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h new file mode 100644 index 000000000..b9ff9814d --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsC_API.h @@ -0,0 +1,20 @@ +#ifndef MUJOCO_PHYSICS_C_API_H +#define MUJOCO_PHYSICS_C_API_H + +#ifdef BT_ENABLE_MUJOCO + +#include "../PhysicsClientC_API.h" + +#ifdef __cplusplus +extern "C" { +#endif + +//think more about naming. The b3ConnectPhysicsLoopback +B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsMuJoCo(); + +#ifdef __cplusplus +} +#endif + +#endif //BT_ENABLE_MUJOCO +#endif //MUJOCO_PHYSICS_C_API_H diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp b/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp new file mode 100644 index 000000000..c78b381a0 --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.cpp @@ -0,0 +1,1598 @@ +#ifdef BT_ENABLE_MUJOCO +#include "MuJoCoPhysicsClient.h" + +#include "../PhysicsClientSharedMemory.h" +#include "../../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemoryCommands.h" +#include "../PhysicsCommandProcessorInterface.h" +#include "../../Utils/b3Clock.h" + +#include "LinearMath/btHashMap.h" +#include "LinearMath/btAlignedObjectArray.h" +#include "../../../Extras/Serialize/BulletFileLoader/btBulletFile.h" +#include "../../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h" +#include "../BodyJointInfoUtility.h" +#include + +#include "../SharedMemoryUserData.h" +#include "LinearMath/btQuickprof.h" + +struct MuJoCoUserDataCache { + btHashMap m_userDataMap; + btHashMap m_keyToUserDataIdMap; + + ~MuJoCoUserDataCache() + { + + } +}; + +struct BodyJointInfoCache2 +{ + std::string m_baseName; + btAlignedObjectArray m_jointInfo; + std::string m_bodyName; + + // Joint index -> user data. + btHashMap m_jointToUserDataMap; + + ~BodyJointInfoCache2() { + } +}; + + + +struct MuJoCoPhysicsDirectInternalData +{ + DummyGUIHelper m_noGfx; + + btAlignedObjectArray m_serverDNA; + SharedMemoryCommand m_command; + SharedMemoryStatus m_serverStatus; + + SharedMemoryCommand m_tmpInfoRequestCommand; + SharedMemoryStatus m_tmpInfoStatus; + bool m_hasStatus; + bool m_verboseOutput; + + btAlignedObjectArray m_debugLinesFrom; + btAlignedObjectArray m_debugLinesTo; + btAlignedObjectArray m_debugLinesColor; + + btHashMap m_bodyJointMap; + btHashMap m_userConstraintInfoMap; + + btAlignedObjectArray m_profileTimings; + btHashMap m_profileTimingStringArray; + + char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE]; + btAlignedObjectArray m_cachedMassMatrix; + int m_cachedCameraPixelsWidth; + int m_cachedCameraPixelsHeight; + btAlignedObjectArray m_cachedCameraPixelsRGBA; + btAlignedObjectArray m_cachedCameraDepthBuffer; + btAlignedObjectArray m_cachedSegmentationMask; + + btAlignedObjectArray m_cachedContactPoints; + btAlignedObjectArray m_cachedOverlappingObjects; + + btAlignedObjectArray m_cachedVisualShapes; + btAlignedObjectArray m_cachedCollisionShapes; + + btAlignedObjectArray m_cachedVREvents; + + btAlignedObjectArray m_cachedKeyboardEvents; + btAlignedObjectArray m_cachedMouseEvents; + + btAlignedObjectArray m_raycastHits; + + PhysicsCommandProcessorInterface* m_commandProcessor; + bool m_ownsCommandProcessor; + double m_timeOutInSeconds; + + MuJoCoPhysicsDirectInternalData() + :m_hasStatus(false), + m_verboseOutput(false), + m_cachedCameraPixelsWidth(0), + m_cachedCameraPixelsHeight(0), + m_commandProcessor(NULL), + m_ownsCommandProcessor(false), + m_timeOutInSeconds(1e30) + { + memset(&m_command, 0, sizeof(m_command)); + memset(&m_serverStatus, 0, sizeof(m_serverStatus)); + memset(m_bulletStreamDataServerToClient, 0, sizeof(m_bulletStreamDataServerToClient)); + } +}; + +MuJoCoPhysicsClient::MuJoCoPhysicsClient(PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership) +{ + int sz = sizeof(SharedMemoryCommand); + int sz2 = sizeof(SharedMemoryStatus); + + m_data = new MuJoCoPhysicsDirectInternalData; + m_data->m_commandProcessor = physSdk; + m_data->m_ownsCommandProcessor = passSdkOwnership; +} + +MuJoCoPhysicsClient::~MuJoCoPhysicsClient() +{ + for (int i=0;im_profileTimingStringArray.size();i++) + { + std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i); + if (str) + { + delete *str; + } + } + m_data->m_profileTimingStringArray.clear(); + + if (m_data->m_commandProcessor->isConnected()) + { + m_data->m_commandProcessor->disconnect(); + } + if (m_data->m_ownsCommandProcessor) + { + delete m_data->m_commandProcessor; + } + + resetData(); + + + + delete m_data; +} + +void MuJoCoPhysicsClient::resetData() +{ + m_data->m_debugLinesFrom.clear(); + m_data->m_debugLinesTo.clear(); + m_data->m_debugLinesColor.clear(); + for (int i = 0; im_bodyJointMap.size(); i++) + { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); + if (bodyJointsPtr && *bodyJointsPtr) + { + delete (*bodyJointsPtr); + } + } + m_data->m_bodyJointMap.clear(); + m_data->m_userConstraintInfoMap.clear(); +} + +// return true if connection succesfull, can also check 'isConnected' +bool MuJoCoPhysicsClient::connect() +{ + bool connected = m_data->m_commandProcessor->connect(); + m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx); + + + if (connected) + //also request serialization data + { + SharedMemoryCommand command; + command.m_type = CMD_REQUEST_INTERNAL_DATA; + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + if (hasStatus) + { + postProcessStatus(m_data->m_serverStatus); + } + else + { + b3Clock clock; + double timeSec = clock.getTimeInSeconds(); + + while ((!hasStatus) && (clock.getTimeInSeconds()-timeSec <10 )) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + } + } + + return connected; +} + +// return true if connection succesfull, can also check 'isConnected' +bool MuJoCoPhysicsClient::connect(struct GUIHelperInterface* guiHelper) +{ + bool connected = m_data->m_commandProcessor->connect(); + + m_data->m_commandProcessor->setGuiHelper(guiHelper); + + return connected; +} + +void MuJoCoPhysicsClient::renderScene() +{ + int renderFlags = 0; + m_data->m_commandProcessor->renderScene(renderFlags); +} + +void MuJoCoPhysicsClient::debugDraw(int debugDrawMode) +{ + m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode); +} + +////todo: rename to 'disconnect' +void MuJoCoPhysicsClient::disconnectSharedMemory() +{ + m_data->m_commandProcessor->disconnect(); + m_data->m_commandProcessor->setGuiHelper(0); +} + +bool MuJoCoPhysicsClient::isConnected() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +// return non-null if there is a status, nullptr otherwise +const SharedMemoryStatus* MuJoCoPhysicsClient::processServerStatus() +{ + + if (!m_data->m_hasStatus) + { + m_data->m_hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + SharedMemoryStatus* stat = 0; + if (m_data->m_hasStatus) + { + stat = &m_data->m_serverStatus; + + postProcessStatus(m_data->m_serverStatus); + + m_data->m_hasStatus = false; + } + return stat; +} + +SharedMemoryCommand* MuJoCoPhysicsClient::getAvailableSharedMemoryCommand() +{ + return &m_data->m_command; +} + +bool MuJoCoPhysicsClient::canSubmitCommand() const +{ + return m_data->m_commandProcessor->isConnected(); +} + +bool MuJoCoPhysicsClient::processDebugLines(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == CMD_DEBUG_LINES_COMPLETED); + + if (m_data->m_verboseOutput) + { + b3Printf("Success receiving %d debug lines", + serverCmd.m_sendDebugLinesArgs.m_numDebugLines); + } + + int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines; + float* linesFrom = + (float*)&m_data->m_bulletStreamDataServerToClient[0]; + float* linesTo = + (float*)(&m_data->m_bulletStreamDataServerToClient[0] + + numLines * 3 * sizeof(float)); + float* linesColor = + (float*)(&m_data->m_bulletStreamDataServerToClient[0] + + 2 * numLines * 3 * sizeof(float)); + + m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + numLines); + m_data->m_debugLinesColor.resize( + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines); + + for (int i = 0; i < numLines; i++) + { + TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1], + linesFrom[i * 3 + 2]); + TmpFloat3 to = + CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]); + TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1], + linesColor[i * 3 + 2]); + + m_data + ->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + from; + m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] = + to; + m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + + i] = color; + } + + if (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0) + { + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_DEBUG_LINES; + command.m_requestDebugLinesArguments.m_startingLineIndex = + serverCmd.m_sendDebugLinesArgs.m_numDebugLines + + serverCmd.m_sendDebugLinesArgs.m_startingLineIndex; + } + } + + } while (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0); + + return m_data->m_hasStatus; +} + +bool MuJoCoPhysicsClient::processVisualShapeData(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + + if (m_data->m_verboseOutput) + { + b3Printf("Visual Shape Information Request OK\n"); + } + int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex; + int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied); + b3VisualShapeData* shapeData = (b3VisualShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numVisualShapesCopied; i++) + { + m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i]; + } + + if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes >0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied) + { + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO; + command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied; + command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId; + } + } + } while (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied); + + return m_data->m_hasStatus; +} + +bool MuJoCoPhysicsClient::processOverlappingObjects(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Overlapping Objects Request OK\n"); + } + + int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex; + int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied); + b3OverlappingObject* objects = (b3OverlappingObject*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i = 0; i < numOverlapCopied; i++) + { + m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i]; + } + + if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied) + { + m_data->m_hasStatus = false; + command.m_type = CMD_REQUEST_AABB_OVERLAP; + command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied; + } + + } + } while (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied); + + return m_data->m_hasStatus; + +} + + + +bool MuJoCoPhysicsClient::processContactPointData(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + if (m_data->m_verboseOutput) + { + b3Printf("Contact Point Information Request OK\n"); + } + int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex; + int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + + m_data->m_cachedContactPoints.resize(startContactIndex+numContactsCopied); + + b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0]; + + for (int i=0;im_cachedContactPoints[startContactIndex+i] = contactData[i]; + } + + if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints>0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied) + { + + m_data->m_hasStatus = false; + + command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION; + command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex+serverCmd.m_sendContactPointArgs.m_numContactPointsCopied; + command.m_requestContactPointArguments.m_objectAIndexFilter = -1; + command.m_requestContactPointArguments.m_objectBIndexFilter = -1; + + } + + } + } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied); + + return m_data->m_hasStatus; + +} + + +bool MuJoCoPhysicsClient::processCamera(const struct SharedMemoryCommand& orgCommand) +{ + SharedMemoryCommand command = orgCommand; + + const SharedMemoryStatus& serverCmd = m_data->m_serverStatus; + + do + { + + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + const SharedMemoryStatus* stat = processServerStatus(); + if (stat) + { + hasStatus = true; + } + } + + + m_data->m_hasStatus = hasStatus; + if (hasStatus) + { + btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED); + + if (m_data->m_verboseOutput) + { + b3Printf("Camera image OK\n"); + } + + int numBytesPerPixel = 4;//RGBA + int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+ + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+ + serverCmd.m_sendPixelDataArguments.m_numRemainingPixels; + + m_data->m_cachedCameraPixelsWidth = 0; + m_data->m_cachedCameraPixelsHeight = 0; + + int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight; + + m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel); + m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels); + m_data->m_cachedSegmentationMask.resize(numTotalPixels); + m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel); + + + unsigned char* rgbaPixelsReceived = + (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0]; + + float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]); + int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]); + + // printf("pixel = %d\n", rgbaPixelsReceived[0]); + + for (int i=0;im_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i]; + } + for (int i=0;im_cachedSegmentationMask[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i]; + } + for (int i=0;im_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel] + = rgbaPixelsReceived[i]; + } + + if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied) + { + + m_data->m_hasStatus = false; + + // continue requesting remaining pixels + command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA; + command.m_requestPixelDataArguments.m_startPixelIndex = + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex + + serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; + + } else + { + m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth; + m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight; + } + } + } while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied); + + return m_data->m_hasStatus; + + +} + + +void MuJoCoPhysicsClient::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) +{ + + BodyJointInfoCache2** cachePtr = m_data->m_bodyJointMap[bodyUniqueId]; + //don't process same bodyUniqueId multiple times + if (cachePtr) + { + return; + } + + bParse::btBulletFile bf( + &m_data->m_bulletStreamDataServerToClient[0], + serverCmd.m_numDataStreamBytes); + if (m_data->m_serverDNA.size()) + { + bf.setFileDNA(false, &m_data->m_serverDNA[0], m_data->m_serverDNA.size()); + } + else + { + bf.setFileDNAisMemoryDNA(); + } + bf.parse(false); + + BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2; + m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints); + bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName; + + for (int i = 0; i < bf.m_multiBodies.size(); i++) + { + int flag = bf.getFlags(); + if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) + { + Bullet::btMultiBodyDoubleData* mb = + (Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i]; + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } else + { + Bullet::btMultiBodyFloatData* mb = + (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i]; + + + if (mb->m_baseName) + { + bodyJoints->m_baseName = mb->m_baseName; + } + addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput); + } + } + if (bf.ok()) + { + if (m_data->m_verboseOutput) + { + b3Printf("Received robot description ok!\n"); + } + } else + { + b3Warning("Robot description not received"); + } +} + +void MuJoCoPhysicsClient::processAddUserData(const struct SharedMemoryStatus& serverCmd) { + const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_userDataResponseArgs.m_userDataGlobalId; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + if (!userDataCachePtr) + { + MuJoCoUserDataCache cache; + (*bodyJointsPtr)->m_jointToUserDataMap.insert(userDataGlobalId.m_linkIndex, cache); + } + userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + + const char *dataStream = m_data->m_bulletStreamDataServerToClient; + + b3UserDataValue userDataValue; + userDataValue.m_type = serverCmd.m_userDataResponseArgs.m_valueType; + userDataValue.m_length = serverCmd.m_userDataResponseArgs.m_valueLength; + SharedMemoryUserData *userDataPtr = userDataCachePtr->m_userDataMap[userDataGlobalId.m_userDataId]; + if (userDataPtr) { + // Only replace the value. + (userDataPtr)->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + } + else { + // Add a new user data entry. + (userDataCachePtr)->m_userDataMap.insert(userDataGlobalId.m_userDataId, SharedMemoryUserData(serverCmd.m_userDataResponseArgs.m_key)); + userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; + userDataPtr->replaceValue(dataStream,serverCmd.m_userDataResponseArgs.m_valueLength,userDataValue.m_type); + (userDataCachePtr)->m_keyToUserDataIdMap.insert(serverCmd.m_userDataResponseArgs.m_key, userDataGlobalId.m_userDataId); + } + } +} + +void MuJoCoPhysicsClient::postProcessStatus(const struct SharedMemoryStatus& serverCmd) +{ + switch (serverCmd.m_type) + { + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Raycast completed"); + } + m_data->m_raycastHits.clear(); + b3RayHitInfo *rayHits = (b3RayHitInfo *)m_data->m_bulletStreamDataServerToClient; + for (int i=0;im_raycastHits.push_back(rayHits[i]); + } + break; + } + case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED: + { + + if (m_data->m_verboseOutput) + { + b3Printf("Request VR Events completed"); + } + m_data->m_cachedVREvents.resize(serverCmd.m_sendVREvents.m_numVRControllerEvents); + for (int i=0;i< serverCmd.m_sendVREvents.m_numVRControllerEvents;i++) + { + m_data->m_cachedVREvents[i] = serverCmd.m_sendVREvents.m_controllerEvents[i]; + } + break; + } + case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED: + { + if (m_data->m_verboseOutput) + { + b3Printf("Request keyboard events completed"); + } + m_data->m_cachedKeyboardEvents.resize(serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents); + for (int i=0;im_cachedKeyboardEvents[i] = serverCmd.m_sendKeyboardEvents.m_keyboardEvents[i]; + } + break; + } + + case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED: + { + B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Request mouse events completed"); + } + m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents); + for (int i=0;im_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i]; + } + break; + } + + case CMD_REQUEST_INTERNAL_DATA_COMPLETED: + { + if (serverCmd.m_numDataStreamBytes) + { + int numStreamBytes = serverCmd.m_numDataStreamBytes; + m_data->m_serverDNA.resize(numStreamBytes); + for (int i = 0; i < numStreamBytes; i++) + { + m_data->m_serverDNA[i] = m_data->m_bulletStreamDataServerToClient[i]; + } + } + break; + } + case CMD_RESET_SIMULATION_COMPLETED: + { + resetData(); + break; + } + + case CMD_USER_CONSTRAINT_INFO_COMPLETED: + case CMD_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid,serverCmd.m_userConstraintResultArgs); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.remove(cid); + break; + } + case CMD_REMOVE_BODY_FAILED: + { + b3Warning("Remove body failed\n"); + break; + } + case CMD_REMOVE_BODY_COMPLETED: + { + for (int i=0;im_userConstraintInfoMap.remove(key); + } + + break; + } + case CMD_CHANGE_USER_CONSTRAINT_COMPLETED: + { + int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId; + b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid]; + if (userConstraintPtr) + { + const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs; + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) + { + userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0]; + userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1]; + userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) + { + userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3]; + userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4]; + userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5]; + userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6]; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE) + { + userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO) + { + userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET) + { + userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP) + { + userConstraintPtr->m_erp = serverConstraint->m_erp; + } + if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK) + { + userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink; + } + } + break; + } + case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED: + { + break; + } + case CMD_SYNC_BODY_INFO_COMPLETED: + case CMD_MJCF_LOADING_COMPLETED: + case CMD_SDF_LOADING_COMPLETED: + { + //we'll stream further info from the physics server + //so serverCmd will be invalid, make a copy + + int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints; + for (int i=0;im_tmpInfoRequestCommand.m_type = CMD_USER_CONSTRAINT; + m_data->m_tmpInfoRequestCommand.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO; + m_data->m_tmpInfoRequestCommand.m_userConstraintArguments.m_userConstraintUniqueId = constraintUid; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + int cid = m_data->m_tmpInfoStatus.m_userConstraintResultArgs.m_userConstraintUniqueId; + m_data->m_userConstraintInfoMap.insert(cid,m_data->m_tmpInfoStatus.m_userConstraintResultArgs); + } + } + + int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies; + for (int i = 0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_BODY_INFO; + m_data->m_tmpInfoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processBodyJointInfo(bodyUniqueId, m_data->m_tmpInfoStatus); + } + } + break; + } + case CMD_CREATE_MULTI_BODY_COMPLETED: + case CMD_URDF_LOADING_COMPLETED: + { + + if (serverCmd.m_numDataStreamBytes > 0) + { + int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId; + processBodyJointInfo(bodyIndex, serverCmd); + } + break; + } + case CMD_BULLET_LOADING_FAILED: + { + b3Warning("Couldn't load .bullet file"); + break; + } + case CMD_BULLET_LOADING_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED: + { + break; + } + + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED: + { + b3Warning("requestOpenGLVisualizeCamera failed"); + break; + } + case CMD_REMOVE_USER_CONSTRAINT_FAILED: + { + b3Warning("removeConstraint failed"); + break; + } + case CMD_CHANGE_USER_CONSTRAINT_FAILED: + { + //b3Warning("changeConstraint failed"); + break; + } + + case CMD_USER_CONSTRAINT_FAILED: + { + b3Warning("createConstraint failed"); + break; + } + + case CMD_CREATE_COLLISION_SHAPE_FAILED: + { + b3Warning("createCollisionShape failed"); + break; + } + case CMD_CREATE_COLLISION_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_VISUAL_SHAPE_FAILED: + { + b3Warning("createVisualShape failed"); + break; + } + case CMD_CREATE_VISUAL_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_MULTI_BODY_FAILED: + { + b3Warning("createMultiBody failed"); + break; + } + case CMD_REQUEST_COLLISION_INFO_COMPLETED: + { + break; + } + case CMD_REQUEST_COLLISION_INFO_FAILED: + { + b3Warning("Request getCollisionInfo failed"); + break; + } + + case CMD_CUSTOM_COMMAND_COMPLETED: + { + break; + } + case CMD_CUSTOM_COMMAND_FAILED: + { + b3Warning("custom plugin command failed"); + break; + } + case CMD_CLIENT_COMMAND_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_FAILED: + { + b3Warning("jacobian calculation failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_FAILED: + { + b3Warning("calculate mass matrix failed"); + break; + } + case CMD_CALCULATED_MASS_MATRIX_COMPLETED: + { + double* matrixData = (double*)&m_data->m_bulletStreamDataServerToClient[0]; + m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount); + for (int i=0;im_cachedMassMatrix[i] = matrixData[i]; + } + break; + } + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: + { + break; + } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: + { + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: + { + break; + } + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: + { + break; + } + case CMD_SAVE_STATE_COMPLETED: + { + break; + } + case CMD_COLLISION_SHAPE_INFO_FAILED: + { + b3Warning("getCollisionShapeData failed"); + break; + } + case CMD_COLLISION_SHAPE_INFO_COMPLETED: + { + B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED"); + if (m_data->m_verboseOutput) + { + b3Printf("Collision Shape Information Request OK\n"); + } + int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes; + m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied); + b3CollisionShapeData* shapeData = (b3CollisionShapeData*)&m_data->m_bulletStreamDataServerToClient[0]; + for (int i = 0; i < numCollisionShapesCopied; i++) + { + m_data->m_cachedCollisionShapes[i] = shapeData[i]; + } + break; + } + case CMD_RESTORE_STATE_FAILED: + { + b3Warning("restoreState failed"); + break; + } + case CMD_RESTORE_STATE_COMPLETED: + { + break; + } + case CMD_BULLET_SAVING_COMPLETED: + { + break; + } + case CMD_LOAD_SOFT_BODY_FAILED: + { + b3Warning("loadSoftBody failed"); + break; + } + case CMD_LOAD_SOFT_BODY_COMPLETED: + { + break; + } + case CMD_SYNC_USER_DATA_FAILED: + { + b3Warning("Synchronizing user data failed."); + break; + } + case CMD_ADD_USER_DATA_FAILED: + { + b3Warning("Adding user data failed (do the specified body and link exist?)"); + break; + } + case CMD_REMOVE_USER_DATA_FAILED: + { + b3Warning("Removing user data failed"); + break; + } + case CMD_ADD_USER_DATA_COMPLETED: + { + processAddUserData(serverCmd); + break; + } + case CMD_SYNC_USER_DATA_COMPLETED: + { + B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED"); + // Remove all cached user data entries. + for(int i=0; im_bodyJointMap.size(); i++) + { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); + if (bodyJointsPtr && *bodyJointsPtr) + { + (*bodyJointsPtr)->m_jointToUserDataMap.clear(); + } + } + const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers; + b3UserDataGlobalIdentifier *identifiers = new b3UserDataGlobalIdentifier[numIdentifiers]; + memcpy(identifiers, &m_data->m_bulletStreamDataServerToClient[0], numIdentifiers * sizeof(b3UserDataGlobalIdentifier)); + + for (int i=0; im_tmpInfoRequestCommand.m_type = CMD_REQUEST_USER_DATA; + m_data->m_tmpInfoRequestCommand.m_userDataRequestArgs = identifiers[i]; + + bool hasStatus = m_data->m_commandProcessor->processCommand(m_data->m_tmpInfoRequestCommand, m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + + b3Clock clock; + double startTime = clock.getTimeInSeconds(); + double timeOutInSeconds = m_data->m_timeOutInSeconds; + + while ((!hasStatus) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds)) + { + hasStatus = m_data->m_commandProcessor->receiveStatus(m_data->m_tmpInfoStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + } + + if (hasStatus) + { + processAddUserData(m_data->m_tmpInfoStatus); + } + } + delete[] identifiers; + break; + } + case CMD_REMOVE_USER_DATA_COMPLETED: + { + const b3UserDataGlobalIdentifier userDataGlobalId = serverCmd.m_removeUserDataResponseArgs; + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[userDataGlobalId.m_bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) { + MuJoCoUserDataCache *userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[userDataGlobalId.m_linkIndex]; + if (userDataCachePtr) + { + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataGlobalId.m_userDataId]; + if (userDataPtr) { + (userDataCachePtr)->m_keyToUserDataIdMap.remove((userDataPtr)->m_key.c_str()); + (userDataCachePtr)->m_userDataMap.remove(userDataGlobalId.m_userDataId); + } + } + } + break; + } + default: + { + //b3Warning("Unknown server status type"); + } + }; + + +} +bool MuJoCoPhysicsClient::submitClientCommand(const struct SharedMemoryCommand& command) +{ + if (command.m_type==CMD_REQUEST_DEBUG_LINES) + { + return processDebugLines(command); + } + + if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA) + { + return processCamera(command); + } + if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION) + { + return processContactPointData(command); + } + + if (command.m_type == CMD_REQUEST_VISUAL_SHAPE_INFO) + { + return processVisualShapeData(command); + } + if (command.m_type == CMD_REQUEST_AABB_OVERLAP) + { + return processOverlappingObjects(command); + } + + bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE); + m_data->m_hasStatus = hasStatus; + /*if (hasStatus) + { + postProcessStatus(m_data->m_serverStatus); + m_data->m_hasStatus = false; + } + */ + return hasStatus; +} + +int MuJoCoPhysicsClient::getNumBodies() const +{ + return m_data->m_bodyJointMap.size(); +} + +void MuJoCoPhysicsClient::removeCachedBody(int bodyUniqueId) +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + delete (*bodyJointsPtr); + m_data->m_bodyJointMap.remove(bodyUniqueId); + } +} + + +int MuJoCoPhysicsClient::getNumUserConstraints() const +{ + return m_data->m_userConstraintInfoMap.size(); +} + +int MuJoCoPhysicsClient::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const +{ + b3UserConstraint* constraintPtr =m_data->m_userConstraintInfoMap[constraintUniqueId]; + if (constraintPtr) + { + info = *constraintPtr; + return 1; + } + return 0; +} + +int MuJoCoPhysicsClient::getUserConstraintId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints())) + { + return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +int MuJoCoPhysicsClient::getBodyUniqueId(int serialIndex) const +{ + if ((serialIndex >= 0) && (serialIndex < getNumBodies())) + { + return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1(); + } + return -1; +} + +bool MuJoCoPhysicsClient::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + strcpy(info.m_baseName,bodyJoints->m_baseName.c_str()); + strcpy(info.m_bodyName ,bodyJoints->m_bodyName .c_str()); + return true; + } + + return false; +} + +int MuJoCoPhysicsClient::getNumJoints(int bodyIndex) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + return bodyJoints->m_jointInfo.size(); + } + btAssert(0); + return 0; +} + +bool MuJoCoPhysicsClient::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const +{ + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex]; + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + if ((jointIndex >=0) && (jointIndex < bodyJoints->m_jointInfo.size())) + { + info = bodyJoints->m_jointInfo[jointIndex]; + return true; + } + } + return false; +} + + +void MuJoCoPhysicsClient::setSharedMemoryKey(int key) +{ +} + + +void MuJoCoPhysicsClient::uploadBulletFileToSharedMemory(const char* data, int len) +{ + if (len>SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) + { + len = SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE; + } + for (int i=0;im_bulletStreamDataServerToClient[i] = data[i]; + } + //m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len); +} + +void MuJoCoPhysicsClient::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) +{ + int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays; + int newNumRays = curNumStreamingRays + numRays; + btAssert(newNumRaysm_bulletStreamDataServerToClient; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1]; + rayDataStream[curNumStreamingRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2]; + command.m_requestRaycastIntersections.m_numStreamingRays++; + } + + } + +} + + +int MuJoCoPhysicsClient::getNumDebugLines() const +{ + return m_data->m_debugLinesFrom.size(); +} + +const float* MuJoCoPhysicsClient::getDebugLinesFrom() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesFrom[0].m_x; + } + return 0; +} +const float* MuJoCoPhysicsClient::getDebugLinesTo() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesTo[0].m_x; + } + return 0; +} +const float* MuJoCoPhysicsClient::getDebugLinesColor() const +{ + if (getNumDebugLines()) + { + return &m_data->m_debugLinesColor[0].m_x; + } + return 0; +} + +void MuJoCoPhysicsClient::getCachedCameraImage(b3CameraImageData* cameraData) +{ + if (cameraData) + { + cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth; + cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight; + cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0; + cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0; + cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0; + } +} + +void MuJoCoPhysicsClient::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) +{ + contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size(); + contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0; + +} + +void MuJoCoPhysicsClient::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects) +{ + overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size(); + overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ? + &m_data->m_cachedOverlappingObjects[0] : 0; +} + + +void MuJoCoPhysicsClient::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo) +{ + visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size(); + visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0; +} + +void MuJoCoPhysicsClient::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo) +{ + collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size(); + collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0; +} + + + +void MuJoCoPhysicsClient::getCachedVREvents(struct b3VREventsData* vrEventsData) +{ + vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size(); + vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents? + &m_data->m_cachedVREvents[0] : 0; +} + +void MuJoCoPhysicsClient::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData) +{ + keyboardEventsData->m_numKeyboardEvents = m_data->m_cachedKeyboardEvents.size(); + keyboardEventsData->m_keyboardEvents = keyboardEventsData->m_numKeyboardEvents? + &m_data->m_cachedKeyboardEvents[0] : 0; +} + +void MuJoCoPhysicsClient::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData) +{ + mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size(); + mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents? + &m_data->m_cachedMouseEvents[0] : 0; +} + + +void MuJoCoPhysicsClient::getCachedRaycastHits(struct b3RaycastInformation* raycastHits) +{ + raycastHits->m_numRayHits = m_data->m_raycastHits.size(); + raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0; +} + +void MuJoCoPhysicsClient::getCachedMassMatrix(int dofCountCheck, double* massMatrix) +{ + int sz = dofCountCheck*dofCountCheck; + if (sz == m_data->m_cachedMassMatrix.size()) + { + for (int i=0;im_cachedMassMatrix[i]; + } + } +} + +void MuJoCoPhysicsClient::setTimeOut(double timeOutInSeconds) +{ + m_data->m_timeOutInSeconds = timeOutInSeconds; +} + +double MuJoCoPhysicsClient::getTimeOut() const +{ + return m_data->m_timeOutInSeconds; +} + +bool MuJoCoPhysicsClient::getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return false; + } + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) + { + return false; + } + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap[userDataId]; + if (!userDataPtr) + { + return false; + } + valueOut.m_type = userDataPtr->m_type; + valueOut.m_length = userDataPtr->m_bytes.size(); + valueOut.m_data1 = userDataPtr->m_bytes.size()? &userDataPtr->m_bytes[0] : 0; + return true; +} + +int MuJoCoPhysicsClient::getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return -1; + } + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) { + return -1; + } + int *userDataId = (userDataCachePtr)->m_keyToUserDataIdMap[key]; + if (!userDataId) { + return -1; + } + return *userDataId; +} + +int MuJoCoPhysicsClient::getNumUserData(int bodyUniqueId, int linkIndex) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + return 0; + } + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr) { + return 0; + } + return (userDataCachePtr)->m_userDataMap.size(); +} + +void MuJoCoPhysicsClient::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId]; + if (!bodyJointsPtr || !(*bodyJointsPtr)) { + *keyOut = 0; + *userDataIdOut = -1; + return; + } + MuJoCoUserDataCache* userDataCachePtr = (*bodyJointsPtr)->m_jointToUserDataMap[linkIndex]; + if (!userDataCachePtr || userDataIndex >= (userDataCachePtr)->m_userDataMap.size()) + { + *keyOut = 0; + *userDataIdOut = -1; + return; + } + *userDataIdOut = (userDataCachePtr)->m_userDataMap.getKeyAtIndex(userDataIndex).getUid1(); + SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex); + *keyOut = (userDataPtr)->m_key.c_str(); +} + + + +void MuJoCoPhysicsClient::pushProfileTiming(const char* timingName) +{ + std::string** strPtr = m_data->m_profileTimingStringArray[timingName]; + std::string* str = 0; + if (strPtr) + { + str = *strPtr; + } else + { + str = new std::string(timingName); + m_data->m_profileTimingStringArray.insert(timingName,str); + } + m_data->m_profileTimings.push_back(new CProfileSample(str->c_str())); +} + + +void MuJoCoPhysicsClient::popProfileTiming() +{ + if (m_data->m_profileTimings.size()) + { + CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size()-1]; + m_data->m_profileTimings.pop_back(); + delete sample; + } +} + +#endif //BT_ENABLE_MUJOCO diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h b/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h new file mode 100644 index 000000000..2c91d8426 --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsClient.h @@ -0,0 +1,123 @@ +#ifndef MUJOCO_PHYSICS_CLIENT_H +#define MUJOCO_PHYSICS_CLIENT_H + +#include "../PhysicsClient.h" + +///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands +class MuJoCoPhysicsClient : public PhysicsClient +{ +protected: + + struct MuJoCoPhysicsDirectInternalData* m_data; + + bool processDebugLines(const struct SharedMemoryCommand& orgCommand); + + bool processCamera(const struct SharedMemoryCommand& orgCommand); + + bool processContactPointData(const struct SharedMemoryCommand& orgCommand); + + bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand); + + bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand); + + void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd); + + void processAddUserData(const struct SharedMemoryStatus& serverCmd); + + void postProcessStatus(const struct SharedMemoryStatus& serverCmd); + + void resetData(); + + void removeCachedBody(int bodyUniqueId); + +public: + + MuJoCoPhysicsClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership); + + virtual ~MuJoCoPhysicsClient(); + + // return true if connection succesfull, can also check 'isConnected' + //it is OK to pass a null pointer for the gui helper + virtual bool connect(); + + ////todo: rename to 'disconnect' + virtual void disconnectSharedMemory(); + + virtual bool isConnected() const; + + // return non-null if there is a status, nullptr otherwise + virtual const SharedMemoryStatus* processServerStatus(); + + virtual SharedMemoryCommand* getAvailableSharedMemoryCommand(); + + virtual bool canSubmitCommand() const; + + virtual bool submitClientCommand(const struct SharedMemoryCommand& command); + + virtual int getNumBodies() const; + + virtual int getBodyUniqueId(int serialIndex) const; + + virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const; + + virtual int getNumJoints(int bodyIndex) const; + + virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const; + + virtual int getNumUserConstraints() const; + + virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const; + + virtual int getUserConstraintId(int serialIndex) const; + + ///todo: move this out of the + virtual void setSharedMemoryKey(int key); + + void uploadBulletFileToSharedMemory(const char* data, int len); + + virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays); + + virtual int getNumDebugLines() const; + + virtual const float* getDebugLinesFrom() const; + virtual const float* getDebugLinesTo() const; + virtual const float* getDebugLinesColor() const; + + virtual void getCachedCameraImage(b3CameraImageData* cameraData); + + virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData); + + virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects); + + virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo); + + virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo); + + virtual void getCachedVREvents(struct b3VREventsData* vrEventsData); + + virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData); + + virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData); + + virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits); + + virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix); + + //the following APIs are for internal use for visualization: + virtual bool connect(struct GUIHelperInterface* guiHelper); + virtual void renderScene(); + virtual void debugDraw(int debugDrawMode); + + virtual void setTimeOut(double timeOutInSeconds); + virtual double getTimeOut() const; + + virtual bool getCachedUserData(int bodyUniqueId, int linkIndex, int userDataId, struct b3UserDataValue &valueOut) const; + virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const; + virtual int getNumUserData(int bodyUniqueId, int linkIndex) const; + virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const; + + virtual void pushProfileTiming(const char* timingName); + virtual void popProfileTiming(); +}; + +#endif //MUJOCO_PHYSICS_CLIENT_H diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp b/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp new file mode 100644 index 000000000..701742fa7 --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.cpp @@ -0,0 +1,1032 @@ +#ifdef BT_ENABLE_MUJOCO +#include "MuJoCoPhysicsServerCommandProcessor.h" +#include "mujoco.h" +#include +#include "../SharedMemoryCommands.h" +#include "LinearMath/btQuickprof.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "LinearMath/btMinMax.h" +#include "Bullet3Common/b3FileUtils.h" +#include "../../Utils/b3ResourcePath.h" + + +struct MuJoCoPhysicsServerCommandProcessorInternalData +{ + bool m_isConnected; + bool m_verboseOutput; + double m_physicsDeltaTime; + int m_numSimulationSubSteps; + + mjModel* m_mujocoModel; + mjData* m_mujocoData; + + b3AlignedObjectArray m_mjcfRecentLoadedBodies; + MuJoCoPhysicsServerCommandProcessorInternalData() + :m_isConnected(false), + m_verboseOutput(false), + m_mujocoModel(0), + m_mujocoData(0), + m_physicsDeltaTime(1./240.), + m_numSimulationSubSteps(0) + { + } +}; + +MuJoCoPhysicsServerCommandProcessor::MuJoCoPhysicsServerCommandProcessor() +{ + m_data = new MuJoCoPhysicsServerCommandProcessorInternalData; +} + +MuJoCoPhysicsServerCommandProcessor::~MuJoCoPhysicsServerCommandProcessor() +{ + delete m_data; + +} + +bool MuJoCoPhysicsServerCommandProcessor::connect() +{ + if (m_data->m_isConnected) + { + printf("already connected\n"); + return true; + } + + printf("MuJoCo Pro library version %.2lf\n", 0.01*mj_version()); + if( mjVERSION_HEADER!=mj_version() ) + mju_error("Headers and library have different versions"); + + // activate MuJoCo license + int result = mj_activate("mjkey.txt"); + if (result==1) + { + m_data->m_isConnected = true; + return true; + } + + return false; +} + +void MuJoCoPhysicsServerCommandProcessor::resetSimulation() +{ + if (m_data->m_mujocoModel) + { + mj_deleteModel(m_data->m_mujocoModel); + m_data->m_mujocoModel=0; + } + if (m_data->m_mujocoData) + { + mj_deleteData(m_data->m_mujocoData); + m_data->m_mujocoData = 0; + } +} + +void MuJoCoPhysicsServerCommandProcessor::disconnect() +{ + resetSimulation(); + + m_data->m_isConnected = false; +} + +bool MuJoCoPhysicsServerCommandProcessor::isConnected() const +{ + return m_data->m_isConnected; +} + +bool MuJoCoPhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + // BT_PROFILE("processCommand"); + + int sz = sizeof(SharedMemoryStatus); + int sz2 = sizeof(SharedMemoryCommand); + + bool hasStatus = false; + + + serverStatusOut.m_type = CMD_INVALID_STATUS; + serverStatusOut.m_numDataStreamBytes = 0; + serverStatusOut.m_dataStream = 0; + + //consume the command + switch (clientCmd.m_type) + { + case CMD_REQUEST_INTERNAL_DATA: + { + hasStatus = processRequestInternalDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + + case CMD_SYNC_BODY_INFO: + { + hasStatus = processSyncBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SYNC_USER_DATA: + { + hasStatus = processSyncUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_MJCF: + { + hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_BODY_INFO: + { + hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_STEP_FORWARD_SIMULATION: + { + hasStatus = processForwardDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processSendPhysicsParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + + }; + + case CMD_REQUEST_ACTUAL_STATE: + { + hasStatus = processRequestActualStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_RESET_SIMULATION: + { + hasStatus = processResetSimulationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + 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; + } + +#if 0 + case CMD_STATE_LOGGING: + { + hasStatus = processStateLoggingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SET_VR_CAMERA_STATE: + { + hasStatus = processSetVRCameraStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VR_EVENTS_DATA: + { + hasStatus = processRequestVREventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_MOUSE_EVENTS_DATA: + { + hasStatus = processRequestMouseEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_KEYBOARD_EVENTS_DATA: + { + hasStatus = processRequestKeyboardEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + + case CMD_REQUEST_RAY_CAST_INTERSECTIONS: + { + + hasStatus = processRequestRaycastIntersectionsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_REQUEST_DEBUG_LINES: + { + hasStatus = processRequestDebugLinesCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_CAMERA_IMAGE_DATA: + { + hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_BODY_INFO: + { + hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_WORLD: + { + hasStatus = processSaveWorldCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_SDF: + { + hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_COLLISION_SHAPE: + { + hasStatus = processCreateCollisionShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_VISUAL_SHAPE: + { + hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_MULTI_BODY: + { + hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SET_ADDITIONAL_SEARCH_PATH: + { + hasStatus = processSetAdditionalSearchPathCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_URDF: + { + hasStatus = processLoadURDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_SOFT_BODY: + { + hasStatus = processLoadSoftBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_SENSOR: + { + hasStatus = processCreateSensorCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PROFILE_TIMING: + { + hasStatus = processProfileTimingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SEND_DESIRED_STATE: + { + hasStatus = processSendDesiredStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_COLLISION_INFO: + { + hasStatus = processRequestCollisionInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + + + case CMD_CHANGE_DYNAMICS_INFO: + { + hasStatus = processChangeDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + }; + case CMD_GET_DYNAMICS_INFO: + { + hasStatus = processGetDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS: + { + hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + + case CMD_INIT_POSE: + { + hasStatus = processInitPoseCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_CREATE_RIGID_BODY: + { + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CREATE_BOX_COLLISION_SHAPE: + { + //for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY + hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_PICK_BODY: + { + hasStatus = processPickBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_MOVE_PICKED_BODY: + { + hasStatus = processMovePickedBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_PICKING_CONSTRAINT_BODY: + { + hasStatus = processRemovePickingConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_AABB_OVERLAP: + { + hasStatus = processRequestAabbOverlapCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA: + { + hasStatus = processRequestOpenGLVisualizeCameraCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CONFIGURE_OPENGL_VISUALIZER: + { + hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_CONTACT_POINT_INFORMATION: + { + hasStatus = processRequestContactpointInformationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_DYNAMICS: + { + hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_JACOBIAN: + { + hasStatus = processCalculateJacobianCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_MASS_MATRIX: + { + hasStatus = processCalculateMassMatrixCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_APPLY_EXTERNAL_FORCE: + { + hasStatus = processApplyExternalForceCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_BODY: + { + hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_USER_CONSTRAINT: + { + hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_CALCULATE_INVERSE_KINEMATICS: + { + hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REQUEST_VISUAL_SHAPE_INFO: + { + 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; + } + case CMD_CHANGE_TEXTURE: + { + hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_LOAD_TEXTURE: + { + hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_RESTORE_STATE: + { + hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_SAVE_STATE: + { + hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_LOAD_BULLET: + { + hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_SAVE_BULLET: + { + 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; + } + case CMD_CUSTOM_COMMAND: + { + hasStatus = processCustomCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + + case CMD_REQUEST_USER_DATA: + { + hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_ADD_USER_DATA: + { + hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } + case CMD_REMOVE_USER_DATA: + { + hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes); + break; + } +#endif + + }; + + return hasStatus; +} + +bool MuJoCoPhysicsServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_REQUEST_INTERNAL_DATA"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED; + serverCmd.m_numDataStreamBytes = 0; + return hasStatus; +} + + + +bool MuJoCoPhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SYNC_BODY_INFO"); + int actualNumBodies = 0; + serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0; + serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0; + serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED; + return hasStatus; +} + +bool MuJoCoPhysicsServerCommandProcessor::processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_SYNC_USER_DATA"); + int numIdentifiers = 0; + serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers; + serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED; + return hasStatus; +} + +bool MuJoCoPhysicsServerCommandProcessor::processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_LOAD_MJCF"); + serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED; + const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments; + if (m_data->m_verboseOutput) + { + printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName); + } + bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody!=0) : true; + int flags = 0; + if (clientCmd.m_updateFlags&URDF_ARGS_HAS_CUSTOM_URDF_FLAGS) + { + flags |= clientCmd.m_mjcfArguments.m_flags; + } + + const char* fileName = mjcfArgs.m_mjcfFileName; + + if (strlen(fileName)>0) + { + char relativeFileName[1024]; + b3FileUtils fu; + //bool fileFound = fu.findFile(fileName, relativeFileName, 1024); + bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0); + if (!fileFound){ + printf("MJCF file not found: %s\n", fileName); + } else + { + int maxPathLen = 1024; + char pathPrefix[1024]; + fu.extractPath(relativeFileName,pathPrefix,maxPathLen); + + { + char error[1000] = "could not load binary model"; + mjModel* mnew = 0; + if( strlen(relativeFileName)>4 && !strcmp(relativeFileName+strlen(relativeFileName)-4, ".mjb") ) + { + mnew = mj_loadModel(relativeFileName, 0); + } + else + { + mnew = mj_loadXML(relativeFileName, 0, error, 1000); + if (mnew) + { + //replace old one for now + if (m_data->m_mujocoModel) + { + mj_deleteModel(m_data->m_mujocoModel); + } + if (m_data->m_mujocoData) + { + mj_deleteData(m_data->m_mujocoData); + } + m_data->m_mujocoModel = mnew; + m_data->m_mujocoData = mj_makeData(m_data->m_mujocoModel); + mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData); + } + } + if( !mnew ) + { + printf("%s\n", error); + } else + { + int maxBodies = btMin(MAX_SDF_BODIES, mnew->nbody); + + serverStatusOut.m_sdfLoadedArgs.m_numBodies = maxBodies; + for (int i=0;im_mujocoModel && sdfInfoArgs.m_bodyUniqueId>=0 && sdfInfoArgs.m_bodyUniqueIdm_mujocoModel->nbody) + { + const char* name = m_data->m_mujocoModel->names+m_data->m_mujocoModel->name_bodyadr[sdfInfoArgs.m_bodyUniqueId]; + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName,name); + } + + serverStatusOut.m_numDataStreamBytes = streamSizeInBytes; + + return hasStatus; + +} + + +bool MuJoCoPhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_STEP_FORWARD_SIMULATION"); + + if (m_data->m_mujocoModel) + { + if (m_data->m_verboseOutput) + { + b3Printf("Step simulation request"); + b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber); + } + + btScalar deltaTimeScaled = m_data->m_physicsDeltaTime; + + if (m_data->m_numSimulationSubSteps > 0) + { + for (int i=0;im_numSimulationSubSteps;i++) + { + m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime/m_data->m_numSimulationSubSteps; + mj_step(m_data->m_mujocoModel,m_data->m_mujocoData); + mj_forward(m_data->m_mujocoModel,m_data->m_mujocoData); + } + } + else + { + m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime; + mj_step(m_data->m_mujocoModel,m_data->m_mujocoData); + mj_forward(m_data->m_mujocoModel,m_data->m_mujocoData); + } + } + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; + return hasStatus; + +} + + +bool MuJoCoPhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + + BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) + { + m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; + } + +#if 0 + if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION) + { + if (clientCmd.m_physSimParamArgs.m_enableConeFriction) + { + m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &=~SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; + } else + { + m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; + } + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0); + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE) + { + gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_WORLD_SPACE)!=0; + gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_JOINT_FRAME)!=0; + } + + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION) + { + m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0); + } + + //see + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS) + { + //these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk + gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY) + { + btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[1], + clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]); + this->m_data->m_dynamicsWorld->setGravity(grav); +#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + m_data->m_dynamicsWorld->getWorldInfo().m_gravity=grav; + +#endif + if (m_data->m_verboseOutput) + { + b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]); + } + + } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS) + { + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) + { + gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_SLOP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT) + { + m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT!=0; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) + { + m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; + } + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse; + } + if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold; + } + + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) + { + m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP) + { + m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM) + { + m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm = clientCmd.m_physSimParamArgs.m_defaultGlobalCFM; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM) + { + m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM; + } + + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) + { + m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; + } + + + + if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING) + { + b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching); + } + +#endif + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; + return hasStatus; +} + +bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + + if (m_data->m_mujocoModel) + { + BT_PROFILE("CMD_REQUEST_ACTUAL_STATE"); + if (m_data->m_verboseOutput) + { + b3Printf("Sending the actual state (Q,U)"); + } + int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; + + if (bodyUniqueId>=0 && bodyUniqueIdm_mujocoModel->nbody) + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; + + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; + serverCmd.m_sendActualStateArgs.m_numLinks = 0;//todo body->m_multiBody->getNumLinks(); + + int totalDegreeOfFreedomQ = 0; + int totalDegreeOfFreedomU = 0; + + if (serverCmd.m_sendActualStateArgs.m_numLinks>= MAX_DEGREE_OF_FREEDOM) + { + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; + hasStatus = true; + return hasStatus; + } + + bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0); + bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0); + + if (computeForwardKinematics || computeLinkVelocities) + { + //todo:check this + mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData); + } + + //always add the base, even for static (non-moving objects) + //so that we can easily move the 'fixed' base when needed + //do we don't use this conditional "if (!mb->hasFixedBase())" + { + int rootLink = 0;//todo check + int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink]; + //assume mjJNT_FREE? + int qposAdr = (m_data->m_mujocoModel->jnt_qposadr+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink]; + mjtNum* pos = m_data->m_mujocoData->xipos+bodyUniqueId*3; + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = 0; + + mjtNum* orn= m_data->m_mujocoData->xquat+bodyUniqueId*4; + mjtNum* cvel=m_data->m_mujocoData->cvel+bodyUniqueId*6; + + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = 0; + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = 1; + + //base position in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = pos[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = pos[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = pos[2]; + + //base orientation, quaternion x,y,z,w, in world space, carthesian + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = orn[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = orn[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = orn[2]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = orn[3]; + totalDegreeOfFreedomQ +=7;//pos + quaternion + + //base linear velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = cvel[3];//mb->getBaseVel()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = cvel[4];//mb->getBaseVel()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = cvel[5];//mb->getBaseVel()[2]; + + //base angular velocity (in world space, carthesian) + serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = cvel[0];//mb->getBaseOmega()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = cvel[1];//mb->getBaseOmega()[1]; + serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = cvel[2];//mb->getBaseOmega()[2]; + totalDegreeOfFreedomU += 6;//3 linear and 3 angular DOF + } + + //btAlignedObjectArray omega; + //btAlignedObjectArray linVel; + + + int numLinks = m_data->m_mujocoModel->body_jntnum[bodyUniqueId]; + for (int l=0;lm_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l]; + //int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l]; + + #if 0 + mjtNum* xpos = + for (int d=0;dgetLink(l).m_posVarCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = 0; + } + for (int d=0;dgetLink(l).m_dofCount;d++) + { + serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = 0; + } + + if (0 == mb->getLink(l).m_jointFeedback) + { + for (int d=0;d<6;d++) + { + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0; + } + } else + { + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = 0; + + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = 0; + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = 0; + } + + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; + #if 0 + if (supportsJointMotor(mb,l)) + { + if (motor && m_data->m_physicsDeltaTime>btScalar(0)) + { + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; + } + } + #endif + //btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); + //btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); + + //btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); + //btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); + + serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = 0;//linkCOMOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = 0;//linkCOMOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = 0;//linkCOMOrigin.getZ(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = 0;//linkCOMRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = 0;//linkCOMRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = 0;//linkCOMRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = 1;//linkCOMRotation.w(); + + + #if 0 + btVector3 worldLinVel(0,0,0); + btVector3 worldAngVel(0,0,0); + + if (computeLinkVelocities) + { + const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis(); + worldLinVel = linkRotMat * linVel[l+1]; + worldAngVel = linkRotMat * omega[l+1]; + } + #endif + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = 0;//worldLinVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = 0;//worldLinVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = 0;//worldLinVel[2]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = 0;//worldAngVel[0]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = 0;//worldAngVel[1]; + serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = 0;//worldAngVel[2]; + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = 0;//linkLocalInertialOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = 0;//linkLocalInertialOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = 0;//linkLocalInertialOrigin.getZ(); + + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = 0;//linkLocalInertialRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = 0;//linkLocalInertialRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = 0;//linkLocalInertialRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = 1;//linkLocalInertialRotation.w(); + #endif + } + + + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; + + hasStatus = true; + + } + } + return hasStatus; +} + + +bool MuJoCoPhysicsServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + bool hasStatus = true; + BT_PROFILE("CMD_RESET_SIMULATION"); + + resetSimulation(); + + + SharedMemoryStatus& serverCmd =serverStatusOut; + serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; + return hasStatus; +} + + + +bool MuJoCoPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) +{ + return false; +} + +#endif //BT_ENABLE_MUJOCO \ No newline at end of file diff --git a/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.h b/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.h new file mode 100644 index 000000000..f4a24d0d1 --- /dev/null +++ b/examples/SharedMemory/mujoco/MuJoCoPhysicsServerCommandProcessor.h @@ -0,0 +1,45 @@ +#ifndef MUJOCO_PHYSICS_SERVER_COMMAND_PROCESSOR_H +#define MUJOCO_PHYSICS_SERVER_COMMAND_PROCESSOR_H + +#include "../PhysicsCommandProcessorInterface.h" + +class MuJoCoPhysicsServerCommandProcessor : public PhysicsCommandProcessorInterface +{ + + struct MuJoCoPhysicsServerCommandProcessorInternalData* m_data; + + bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + void resetSimulation(); + +public: + MuJoCoPhysicsServerCommandProcessor(); + + virtual ~MuJoCoPhysicsServerCommandProcessor(); + + virtual bool connect(); + + virtual void disconnect(); + + virtual bool isConnected() const; + + virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes); + + virtual void renderScene(int renderFlags){} + virtual void physicsDebugDraw(int debugDrawFlags){} + virtual void setGuiHelper(struct GUIHelperInterface* guiHelper){} + virtual void setTimeOut(double timeOutInSeconds){} + +}; + +#endif //MUJOCO_PHYSICS_COMMAND_PROCESSOR_H diff --git a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp index 11f207132..132cc2def 100644 --- a/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp @@ -671,13 +671,15 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( visualShape.m_rgbaColor[1] = rgbaColor[1]; visualShape.m_rgbaColor[2] = rgbaColor[2]; visualShape.m_rgbaColor[3] = rgbaColor[3]; + visualShape.m_openglTextureId = -1; + visualShape.m_tinyRendererTextureId = -1; + visualShape.m_textureUniqueId = -1; { B3_PROFILE("convertURDFToVisualShape"); convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices, textures, visualShape); } - m_data->m_visualShapes.push_back(visualShape); - + if (vertices.size() && indices.size()) { TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_shadowBuffer, &m_data->m_segmentationMaskBuffer, bodyUniqueId, linkIndex); @@ -701,13 +703,15 @@ void TinyRendererVisualShapeConverter::convertVisualShapes( } visuals->m_renderObjects.push_back(tinyObj); } + + btAssert(textures.size()<=1); for (int i=0;im_textures.size(); + m_data->m_textures.push_back(textures[i]); } + m_data->m_visualShapes.push_back(visualShape); + } } } @@ -1199,13 +1203,13 @@ void TinyRendererVisualShapeConverter::changeShapeTexture(int objectUniqueId, in for (int v = 0; v < visualArray->m_renderObjects.size(); v++) { TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v]; + if ((shapeIndex < 0) || (shapeIndex == v)) { renderObj->m_model->setDiffuseTextureFromData(m_data->m_textures[textureUniqueId].textureData1, m_data->m_textures[textureUniqueId].m_width, m_data->m_textures[textureUniqueId].m_height); } } } - } } } diff --git a/examples/SharedMemory/tcp/main.cpp b/examples/SharedMemory/tcp/main.cpp index 6c2df155d..5bff8b464 100644 --- a/examples/SharedMemory/tcp/main.cpp +++ b/examples/SharedMemory/tcp/main.cpp @@ -177,17 +177,30 @@ int main(int argc, char *argv[]) SharedMemoryCommand cmd; SharedMemoryCommand* cmdPtr = 0; - + + int type = *(int*)&bytesReceived[0]; + //performance test if (numBytesRec == sizeof(int)) { cmdPtr = &cmd; cmd.m_type = *(int*)&bytesReceived[0]; } - - if (numBytesRec == sizeof(SharedMemoryCommand)) + else { - cmdPtr = (SharedMemoryCommand*)&bytesReceived[0]; + + if (numBytesRec == sizeof(SharedMemoryCommand)) + { + cmdPtr = (SharedMemoryCommand*)&bytesReceived[0]; + } + else + { + if (numBytesRec==36) + { + cmdPtr = &cmd; + memcpy(&cmd, &bytesReceived[0], numBytesRec); + } + } } if (cmdPtr) { @@ -207,7 +220,7 @@ int main(int argc, char *argv[]) } if (gVerboseNetworkMessagesServer) { - printf("buffer.size = %d\n", buffer.size()); + //printf("buffer.size = %d\n", buffer.size()); printf("serverStatus.m_numDataStreamBytes = %d\n", serverStatus.m_numDataStreamBytes); } if (hasStatus) @@ -234,25 +247,49 @@ int main(int argc, char *argv[]) } else { - //create packetData with [int packetSizeInBytes, status, streamBytes) - packetData.resize(4 + sizeof(SharedMemoryStatus) + serverStatus.m_numDataStreamBytes); - int sz = packetData.size(); - int curPos = 0; - - MySerializeInt(sz, &packetData[curPos]); - curPos += 4; - for (int i = 0; i < sizeof(SharedMemoryStatus); i++) + if (cmdPtr->m_type == CMD_REQUEST_VR_EVENTS_DATA) { - packetData[i + curPos] = statBytes[i]; + int headerSize = 16+5 * sizeof(int) + sizeof(smUint64_t) + sizeof(char*) + sizeof(b3VRControllerEvent)*serverStatus.m_sendVREvents.m_numVRControllerEvents; + packetData.resize(4 + headerSize); + int sz = packetData.size(); + int curPos = 0; + MySerializeInt(sz, &packetData[curPos]); + curPos += 4; + for (int i = 0; i < headerSize; i++) + { + packetData[i + curPos] = statBytes[i]; + } + curPos += headerSize; + pClient->Send(&packetData[0], packetData.size()); } - curPos += sizeof(SharedMemoryStatus); - - for (int i = 0; i < serverStatus.m_numDataStreamBytes; i++) + else { - packetData[i + curPos] = buffer[i]; + //create packetData with [int packetSizeInBytes, status, streamBytes) + packetData.resize(4 + sizeof(SharedMemoryStatus) + serverStatus.m_numDataStreamBytes); + int sz = packetData.size(); + int curPos = 0; + + if (gVerboseNetworkMessagesServer) + { + //printf("buffer.size = %d\n", buffer.size()); + printf("serverStatus packed size = %d\n", sz); + } + + MySerializeInt(sz, &packetData[curPos]); + curPos += 4; + for (int i = 0; i < sizeof(SharedMemoryStatus); i++) + { + packetData[i + curPos] = statBytes[i]; + } + curPos += sizeof(SharedMemoryStatus); + + for (int i = 0; i < serverStatus.m_numDataStreamBytes; i++) + { + packetData[i + curPos] = buffer[i]; + } + + pClient->Send(&packetData[0], packetData.size()); } - - pClient->Send( &packetData[0], packetData.size() ); } } diff --git a/examples/pybullet/examples/biped2d_pybullet.py b/examples/pybullet/examples/biped2d_pybullet.py new file mode 100644 index 000000000..01d3aaec9 --- /dev/null +++ b/examples/pybullet/examples/biped2d_pybullet.py @@ -0,0 +1,40 @@ +import pybullet as p +import pybullet_data +import os +import time +GRAVITY = -9.8 +dt = 1e-3 +iters=2000 + +physicsClient = p.connect(p.GUI) +p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.resetSimulation() +#p.setRealTimeSimulation(True) +p.setGravity(0,0,GRAVITY) +p.setTimeStep(dt) +planeId = p.loadURDF("plane.urdf") +cubeStartPos = [0,0,1.13] +cubeStartOrientation = p.getQuaternionFromEuler([0.,0,0]) +botId = p.loadURDF("biped/biped2d_pybullet.urdf", + cubeStartPos, + cubeStartOrientation) + +#disable the default velocity motors +#and set some position control with small force to emulate joint friction/return to a rest pose +jointFrictionForce=1 +for joint in range (p.getNumJoints(botId)): + p.setJointMotorControl2(botId,joint,p.POSITION_CONTROL,force=jointFrictionForce) + +#for i in range(10000): +# p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0) +# p.stepSimulation() +#import ipdb +#ipdb.set_trace() +import time +p.setRealTimeSimulation(1) +while (1): + #p.stepSimulation() + #p.setJointMotorControl2(botId, 1, p.TORQUE_CONTROL, force=1098.0) + p.setGravity(0,0,GRAVITY) + time.sleep(1/240.) +time.sleep(1000) diff --git a/examples/pybullet/examples/getTextureUid.py b/examples/pybullet/examples/getTextureUid.py new file mode 100644 index 000000000..50eda65e9 --- /dev/null +++ b/examples/pybullet/examples/getTextureUid.py @@ -0,0 +1,19 @@ +import pybullet as p +p.connect(p.GUI) +plane = p.loadURDF("plane.urdf") +visualData = p.getVisualShapeData(plane, p.VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS) +print(visualData) +curTexUid = visualData[0][8] +print(curTexUid) +texUid = p.loadTexture("tex256.png") +print("texUid=",texUid) + +p.changeVisualShape(plane,-1,textureUniqueId=texUid) + +for i in range (100): + p.getCameraImage(320,200) +p.changeVisualShape(plane,-1,textureUniqueId=curTexUid) + +for i in range (100): + p.getCameraImage(320,200) + diff --git a/examples/pybullet/examples/otherPhysicsEngine.py b/examples/pybullet/examples/otherPhysicsEngine.py new file mode 100644 index 000000000..2786b1c66 --- /dev/null +++ b/examples/pybullet/examples/otherPhysicsEngine.py @@ -0,0 +1,32 @@ +import pybullet as p +import time + +#p.connect(p.DIRECT) +#p.connect(p.DART) +p.connect(p.MuJoCo) + +#p.connect(p.GUI) +bodies = p.loadMJCF("mjcf/capsule.xml") +print("bodies=",bodies) + +numBodies = p.getNumBodies() +print("numBodies=",numBodies) +for i in range (numBodies): + print("bodyInfo[",i,"]=",p.getBodyInfo(i)) + +p.setGravity(0,0,-10) +timeStep = 1./240. +p.setPhysicsEngineParameter(fixedTimeStep=timeStep) + +#while (p.isConnected()): +for i in range (1000): + p.stepSimulation() + + for b in bodies: + pos,orn=p.getBasePositionAndOrientation(b) + print("pos[",b,"]=",pos) + print("orn[",b,"]=",orn) + linvel,angvel=p.getBaseVelocity(b) + print("linvel[",b,"]=",linvel) + print("angvel[",b,"]=",angvel) + time.sleep(timeStep) diff --git a/examples/pybullet/examples/projective_texture.py b/examples/pybullet/examples/projective_texture.py index c4b21b9d0..5c3f2ffb5 100644 --- a/examples/pybullet/examples/projective_texture.py +++ b/examples/pybullet/examples/projective_texture.py @@ -13,8 +13,8 @@ bearStartPos2 = [0,0,0] bearStartOrientation2 = p.getQuaternionFromEuler([0,0,0]) bearId2 = p.loadURDF("teddy_large.urdf",bearStartPos2, bearStartOrientation2) textureId = p.loadTexture("checker_grid.jpg") -p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId) -p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId) +#p.changeVisualShape(objectUniqueId=0, linkIndex=-1, textureUniqueId=textureId) +#p.changeVisualShape(objectUniqueId=1, linkIndex=-1, textureUniqueId=textureId) useRealTimeSimulation = 1 diff --git a/examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf b/examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf new file mode 100644 index 000000000..e440a65cd --- /dev/null +++ b/examples/pybullet/gym/pybullet_data/biped/biped2d_pybullet.urdf @@ -0,0 +1,273 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b7c704305..5d5ceb79f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5,6 +5,14 @@ #include "../SharedMemory/PhysicsClientUDP_C_API.h" #endif //BT_ENABLE_ENET +#ifdef BT_ENABLE_DART +#include "../SharedMemory/dart/DARTPhysicsC_API.h" +#endif + +#ifdef BT_ENABLE_MUJOCO +#include "../SharedMemory/mujoco/MuJoCoPhysicsC_API.h" +#endif + #ifdef BT_ENABLE_CLSOCKET #include "../SharedMemory/PhysicsClientTCP_C_API.h" #endif //BT_ENABLE_CLSOCKET @@ -407,6 +415,22 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P sm = b3ConnectPhysicsDirect(); break; } +#ifdef BT_ENABLE_DART + case eCONNECT_DART: + { + sm = b3ConnectPhysicsDART(); + break; + } +#endif + +#ifdef BT_ENABLE_MUJOCO + case eCONNECT_MUJOCO: + { + sm = b3ConnectPhysicsMuJoCo(); + break; + } +#endif + case eCONNECT_SHARED_MEMORY: { sm = b3ConnectSharedMemory(key); @@ -5428,8 +5452,10 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyObject* pyResultList = 0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"objectUniqueId", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &objectUniqueId, &physicsClientId)) + int flags=0; + + static char* kwlist[] = {"objectUniqueId", "flags", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|ii", kwlist, &objectUniqueId, &flags, &physicsClientId)) { return NULL; } @@ -5450,7 +5476,9 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes); for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++) { - PyObject* visualShapeObList = PyTuple_New(8); + int numFields = flags&eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS ? 9 : 8; + + PyObject* visualShapeObList = PyTuple_New(numFields); PyObject* item; item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId); PyTuple_SetItem(visualShapeObList, 0, item); @@ -5511,6 +5539,11 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO PyTuple_SetItem(rgba, 3, item); PyTuple_SetItem(visualShapeObList, 7, rgba); } + if (flags&eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS) + { + item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_textureUniqueId); + PyTuple_SetItem(visualShapeObList, 8, item); + } PyTuple_SetItem(pyResultList, i, visualShapeObList); } @@ -9549,6 +9582,14 @@ initpybullet(void) PyModule_AddIntConstant(m, "GUI_SERVER", eCONNECT_GUI_SERVER); // user read PyModule_AddIntConstant(m, "GUI_MAIN_THREAD", eCONNECT_GUI_MAIN_THREAD); // user read PyModule_AddIntConstant(m, "SHARED_MEMORY_SERVER", eCONNECT_SHARED_MEMORY_SERVER); // user read +#ifdef BT_ENABLE_DART + PyModule_AddIntConstant(m, "DART", eCONNECT_DART); // user read +#endif + +#ifdef BT_ENABLE_MUJOCO + PyModule_AddIntConstant(m, "MuJoCo", eCONNECT_MUJOCO); // user read +#endif + PyModule_AddIntConstant(m, "SHARED_MEMORY_KEY", SHARED_MEMORY_KEY); PyModule_AddIntConstant(m, "SHARED_MEMORY_KEY2", SHARED_MEMORY_KEY+1); @@ -9656,6 +9697,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS", URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS); + PyModule_AddIntConstant(m, "VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS", eVISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS); + PyModule_AddIntConstant(m, "MAX_RAY_INTERSECTION_BATCH_SIZE", MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING); PyModule_AddIntConstant(m, "B3G_F1", B3G_F1);