From 0af0f193eef3bfc34873fe2bbba36faa74ce017f Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 6 Mar 2019 23:27:59 -0800 Subject: [PATCH] reduce size of SharedMemoryStatus by moving state details into shared memory streaming block. --- .../MinitaurSimulatorExample.cpp | 147 ++++++++++++++++++ .../RobotSimulator/MinitaurSimulatorExample.h | 22 +++ .../RoboticsLearning/GripperGraspExample.cpp | 4 + .../RoboticsLearning/KukaGraspExample.cpp | 3 + .../RoboticsLearning/R2D2GraspExample.cpp | 3 + examples/SharedMemory/PhysicsClientC_API.cpp | 49 +++--- .../PhysicsClientSharedMemory.cpp | 22 ++- examples/SharedMemory/PhysicsDirect.cpp | 5 + examples/SharedMemory/PhysicsLoopBack.cpp | 3 + .../PhysicsServerCommandProcessor.cpp | 136 ++++++++-------- .../SharedMemoryCommandProcessor.cpp | 11 +- examples/SharedMemory/SharedMemoryCommands.h | 5 + .../SharedMemory/grpc/ConvertGRPCBullet.cpp | 16 +- 13 files changed, 330 insertions(+), 96 deletions(-) create mode 100644 examples/RobotSimulator/MinitaurSimulatorExample.cpp create mode 100644 examples/RobotSimulator/MinitaurSimulatorExample.h diff --git a/examples/RobotSimulator/MinitaurSimulatorExample.cpp b/examples/RobotSimulator/MinitaurSimulatorExample.cpp new file mode 100644 index 000000000..c4da48367 --- /dev/null +++ b/examples/RobotSimulator/MinitaurSimulatorExample.cpp @@ -0,0 +1,147 @@ +#include "MinitaurSimulatorExample.h" +#include "MinitaurSetup.h" + +#include "../CommonInterfaces/CommonGraphicsAppInterface.h" +#include "Bullet3Common/b3Quaternion.h" +#include "Bullet3Common/b3AlignedObjectArray.h" +#include "../CommonInterfaces/CommonRenderInterface.h" +#include "../CommonInterfaces/CommonExampleInterface.h" +#include "../CommonInterfaces/CommonGUIHelperInterface.h" +#include "../SharedMemory/PhysicsServerSharedMemory.h" +#include "../SharedMemory/SharedMemoryPublic.h" + +#include "../CommonInterfaces/CommonParameterInterface.h" +#include "../SharedMemory/PhysicsClientC_API.h" +#include + +#include "../RobotSimulator/b3RobotSimulatorClientAPI.h" +#include "../Utils/b3Clock.h" + +///quick demo showing the right-handed coordinate system and positive rotations around each axis +class MinitaurSimulatorExample : public CommonExampleInterface +{ + CommonGraphicsApp* m_app; + GUIHelperInterface* m_guiHelper; + b3RobotSimulatorClientAPI m_robotSim; + + + int m_options; + + double m_time; + + double m_gravityAccelerationZ; + + MinitaurSetup m_minitaur; + int m_minitaurUid; + + + +public: + MinitaurSimulatorExample(GUIHelperInterface* helper, int options) + : m_app(helper->getAppInterface()), + m_guiHelper(helper), + m_options(options), + m_gravityAccelerationZ(-10), + m_minitaurUid(-1) + { + m_app->setUpAxis(2); + } + virtual ~MinitaurSimulatorExample() + { + } + + virtual void physicsDebugDraw(int debugDrawMode) + { + m_robotSim.debugDraw(debugDrawMode); + } + + virtual void initPhysics() + { + int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; + m_robotSim.setGuiHelper(m_guiHelper); + bool connected = m_robotSim.connect(mode); + //hide the perception camera views for rbd, depth and segmentation mask + m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); + b3Printf("robotSim connected = %d", connected); + + SliderParams slider("Gravity", &m_gravityAccelerationZ); + slider.m_minVal = -10; + slider.m_maxVal = 10; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + + //when in the debugger, don't crash when a command isn't processed immediately, give it 10 seconds + m_robotSim.setTimeOut(10); + + m_robotSim.loadURDF("plane.urdf"); + + m_minitaurUid = m_minitaur.setupMinitaur(&m_robotSim, btVector3(0, 0, .3)); + + + { + b3RobotSimulatorLoadUrdfFileArgs args; + args.m_startPosition.setValue(0, 0, 1); + args.m_startOrientation.setEulerZYX(0, 0, 0); + args.m_useMultiBody = true; + m_robotSim.loadURDF("cube_small.urdf", args); + } + } + + virtual void exitPhysics() + { + m_robotSim.disconnect(); + } + + virtual void stepSimulation(float deltaTime) + { + m_robotSim.setGravity(btVector3(0, 0, m_gravityAccelerationZ)); + m_robotSim.stepSimulation(); + for (int i = 0; i < m_robotSim.getNumJoints(m_minitaurUid);i++) + { + b3JointSensorState state; + m_robotSim.getJointState(this->m_minitaurUid, i, &state); + } + + b3JointStates2 states; + m_robotSim.getJointStates(m_minitaurUid, states); + + } + virtual void renderScene() + { + m_robotSim.renderScene(); + } + + virtual bool mouseMoveCallback(float x, float y) + { + return m_robotSim.mouseMoveCallback(x, y); + } + virtual bool mouseButtonCallback(int button, int state, float x, float y) + { + return m_robotSim.mouseButtonCallback(button, state, x, y); + } + virtual bool keyboardCallback(int key, int state) + { + return false; + } + + virtual void resetCamera() + { + float dist = 1.5; + float pitch = -10; + float yaw = 18; + float targetPos[3] = {-0.2, 0.8, 0.3}; + if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) + { + m_app->m_renderer->getActiveCamera()->setCameraDistance(dist); + m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch); + m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw); + m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0], targetPos[1], targetPos[2]); + } + } + +}; +class CommonExampleInterface* MinitaurSimulatorExampleCreateFunc(struct CommonExampleOptions& options) +{ + return new MinitaurSimulatorExample(options.m_guiHelper, options.m_option); +} diff --git a/examples/RobotSimulator/MinitaurSimulatorExample.h b/examples/RobotSimulator/MinitaurSimulatorExample.h new file mode 100644 index 000000000..bfa940c78 --- /dev/null +++ b/examples/RobotSimulator/MinitaurSimulatorExample.h @@ -0,0 +1,22 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2019 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef MINITAUR_SIMULATOR_EXAMPLE_H +#define MINITAUR_SIMULATOR_EXAMPLE_H + + +class CommonExampleInterface* MinitaurSimulatorExampleCreateFunc(struct CommonExampleOptions& options); + +#endif //MINITAUR_SIMULATOR_EXAMPLE_H diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 843a53ab6..a7c80c1f4 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -62,6 +62,10 @@ public: m_robotSim.setGuiHelper(m_guiHelper); bool connected = m_robotSim.connect(mode); + m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); + b3Printf("robotSim connected = %d", connected); if ((m_options & eGRIPPER_GRASP) != 0) diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 14b6fcb75..831be3659 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -72,6 +72,9 @@ public: int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; m_robotSim.setGuiHelper(m_guiHelper); bool connected = m_robotSim.connect(mode); + m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); // 0;//m_robotSim.connect(m_guiHelper); b3Printf("robotSim connected = %d", connected); diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index 28f5f19cc..f665d5081 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -51,6 +51,9 @@ public: int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; m_robotSim.setGuiHelper(m_guiHelper); bool connected = m_robotSim.connect(mode); + m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0); + m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0); b3Printf("robotSim connected = %d", connected); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 2063722c8..cc2187ed5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -990,6 +990,10 @@ B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMe B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState* state) { + + + + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; b3Assert(status); int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId; @@ -997,13 +1001,14 @@ B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemo if (bodyIndex >= 0) { b3JointInfo info; + bool result = b3GetJointInfo(physClient, bodyIndex, jointIndex, &info) != 0; - if (result) + if (result && status->m_sendActualStateArgs.m_stateDetails) { if ((info.m_qIndex >= 0) && (info.m_uIndex >= 0) && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM)) { - state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex]; - state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex]; + state->m_jointPosition = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQ[info.m_qIndex]; + state->m_jointVelocity = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQdot[info.m_uIndex]; } else { @@ -1012,9 +1017,9 @@ B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemo } for (int ii(0); ii < 6; ++ii) { - state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii]; + state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_stateDetails->m_jointReactionForces[6 * jointIndex + ii]; } - state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex]; + state->m_jointMotorTorque = status->m_sendActualStateArgs.m_stateDetails->m_jointMotorForce[jointIndex]; return 1; } } @@ -1042,12 +1047,12 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh state->m_uDofSize = info.m_uSize; for (int i = 0; i < state->m_qDofSize; i++) { - state->m_jointPosition[i] = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex + i]; + state->m_jointPosition[i] = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQ[info.m_qIndex + i]; } for (int i = 0; i < state->m_uDofSize; i++) { - state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex+i]; - state->m_jointMotorTorqueMultiDof[i] = status->m_sendActualStateArgs.m_jointMotorForceMultiDof[info.m_uIndex + i]; + state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQdot[info.m_uIndex+i]; + state->m_jointMotorTorqueMultiDof[i] = status->m_sendActualStateArgs.m_stateDetails->m_jointMotorForceMultiDof[info.m_uIndex + i]; } } else @@ -1057,7 +1062,7 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh } for (int ii(0); ii < 6; ++ii) { - state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii]; + state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_stateDetails->m_jointReactionForces[6 * jointIndex + ii]; } return 1; @@ -1083,15 +1088,15 @@ B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemor for (int i = 0; i < 3; ++i) { - state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i]; - state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + i]; - state->m_worldLinearVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6 * linkIndex + i]; - state->m_worldAngularVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6 * linkIndex + i + 3]; + state->m_worldPosition[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkState[7 * linkIndex + i]; + state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[7 * linkIndex + i]; + state->m_worldLinearVelocity[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkWorldVelocities[6 * linkIndex + i]; + state->m_worldAngularVelocity[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkWorldVelocities[6 * linkIndex + i + 3]; } for (int i = 0; i < 4; ++i) { - state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i]; - state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i]; + state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkState[7 * linkIndex + 3 + i]; + state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[7 * linkIndex + 3 + i]; } com.setOrigin(b3MakeVector3(state->m_worldPosition[0], state->m_worldPosition[1], state->m_worldPosition[2])); com.setRotation(b3Quaternion(state->m_worldOrientation[0], state->m_worldOrientation[1], state->m_worldOrientation[2], state->m_worldOrientation[3])); @@ -2338,19 +2343,19 @@ B3_SHARED_API int b3GetStatusActualState2(b3SharedMemoryStatusHandle statusHandl } if (linkLocalInertialFrames) { - *linkLocalInertialFrames = args.m_linkLocalInertialFrames; + *linkLocalInertialFrames = args.m_stateDetails->m_linkLocalInertialFrames; } if (jointMotorForces) { - *jointMotorForces = args.m_jointMotorForce; + *jointMotorForces = args.m_stateDetails->m_jointMotorForce; } if (linkStates) { - *linkStates = args.m_linkState; + *linkStates = args.m_stateDetails->m_linkState; } if (linkWorldVelocities) { - *linkWorldVelocities = args.m_linkWorldVelocities; + *linkWorldVelocities = args.m_stateDetails->m_linkWorldVelocities; } return 1; } @@ -2391,15 +2396,15 @@ B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle } if (actualStateQ) { - *actualStateQ = args.m_actualStateQ; + *actualStateQ = args.m_stateDetails->m_actualStateQ; } if (actualStateQdot) { - *actualStateQdot = args.m_actualStateQdot; + *actualStateQdot = args.m_stateDetails->m_actualStateQdot; } if (jointReactionForces) { - *jointReactionForces = args.m_jointReactionForces; + *jointReactionForces = args.m_stateDetails->m_jointReactionForces; } return true; } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 7ba80e823..168d862da 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -70,6 +70,8 @@ struct PhysicsClientSharedMemoryInternalData SharedMemoryStatus m_lastServerStatus; + SendActualStateSharedMemoryStorage m_cachedState; + int m_counter; bool m_isConnected; @@ -79,6 +81,8 @@ struct PhysicsClientSharedMemoryInternalData bool m_verboseOutput; double m_timeOutInSeconds; + + PhysicsClientSharedMemoryInternalData() : m_sharedMemory(0), m_ownsSharedMemory(false), @@ -524,6 +528,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() m_data->m_testBlock1->m_numProcessedServerCommands + 1); const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; + + if (serverCmd.m_type==CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + SendActualStateSharedMemoryStorage* serverState = (SendActualStateSharedMemoryStorage*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; + m_data->m_cachedState = *serverState; + //ideally we provided a 'getCachedState' but that would require changing the API, so we store a pointer instead. + m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_stateDetails = &m_data->m_cachedState; + } + m_data->m_lastServerStatus = serverCmd; // EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type; @@ -827,6 +840,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_COMPLETED"); + if (m_data->m_verboseOutput) { b3Printf("Received actual state\n"); @@ -845,12 +859,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() if (i < numQ - 1) { sprintf(msg, "%s%f,", msg, - command.m_sendActualStateArgs.m_actualStateQ[i]); + m_data->m_cachedState.m_actualStateQ[i]); } else { sprintf(msg, "%s%f", msg, - command.m_sendActualStateArgs.m_actualStateQ[i]); + m_data->m_cachedState.m_actualStateQ[i]); } } sprintf(msg, "%s]", msg); @@ -864,12 +878,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() if (i < numU - 1) { sprintf(msg, "%s%f,", msg, - command.m_sendActualStateArgs.m_actualStateQdot[i]); + m_data->m_cachedState.m_actualStateQdot[i]); } else { sprintf(msg, "%s%f", msg, - command.m_sendActualStateArgs.m_actualStateQdot[i]); + m_data->m_cachedState.m_actualStateQdot[i]); } } sprintf(msg, "%s]", msg); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 7f6c2ec6a..fb5cc81e1 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -79,6 +79,8 @@ struct PhysicsDirectInternalData bool m_ownsCommandProcessor; double m_timeOutInSeconds; + SendActualStateSharedMemoryStorage m_cachedState; + PhysicsDirectInternalData() : m_hasStatus(false), m_verboseOutput(false), @@ -1014,6 +1016,9 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } case CMD_ACTUAL_STATE_UPDATE_COMPLETED: { + SendActualStateSharedMemoryStorage* serverState = (SendActualStateSharedMemoryStorage*)&m_data->m_bulletStreamDataServerToClient[0]; + m_data->m_cachedState = *serverState; + m_data->m_serverStatus.m_sendActualStateArgs.m_stateDetails = &m_data->m_cachedState; break; } case CMD_DESIRED_STATE_RECEIVED_COMPLETED: diff --git a/examples/SharedMemory/PhysicsLoopBack.cpp b/examples/SharedMemory/PhysicsLoopBack.cpp index 4ef7e6c8e..1ad8d43eb 100644 --- a/examples/SharedMemory/PhysicsLoopBack.cpp +++ b/examples/SharedMemory/PhysicsLoopBack.cpp @@ -173,6 +173,9 @@ void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData) return m_data->m_physicsClient->getCachedCameraImage(cameraData); } + + + void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData) { return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 50bb4adef..0c21c325e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6326,15 +6326,26 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + //we store the state details in the shared memory block, to reduce status size + SendActualStateSharedMemoryStorage* stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient; + + //make sure the storage fits, otherwise + btAssert(sizeof(SendActualStateSharedMemoryStorage) < bufferSizeInBytes); + if (sizeof(SendActualStateSharedMemoryStorage) > bufferSizeInBytes) + { + //this forces an error report + body = 0; + } if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; SharedMemoryStatus& serverCmd = serverStatusOut; + serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks(); - + serverCmd.m_sendActualStateArgs.m_stateDetails = 0; int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomU = 0; @@ -6370,26 +6381,26 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc body->m_rootLocalInertialFrame.getRotation()[3]; //base position in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; + stateDetails->m_actualStateQ[0] = tr.getOrigin()[0]; + stateDetails->m_actualStateQ[1] = tr.getOrigin()[1]; + stateDetails->m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; + stateDetails->m_actualStateQ[3] = tr.getRotation()[0]; + stateDetails->m_actualStateQ[4] = tr.getRotation()[1]; + stateDetails->m_actualStateQ[5] = tr.getRotation()[2]; + stateDetails->m_actualStateQ[6] = tr.getRotation()[3]; totalDegreeOfFreedomQ += 7; //pos + quaternion //base linear velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; + stateDetails->m_actualStateQdot[0] = mb->getBaseVel()[0]; + stateDetails->m_actualStateQdot[1] = mb->getBaseVel()[1]; + stateDetails->m_actualStateQdot[2] = mb->getBaseVel()[2]; //base angular velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2]; + stateDetails->m_actualStateQdot[3] = mb->getBaseOmega()[0]; + stateDetails->m_actualStateQdot[4] = mb->getBaseOmega()[1]; + stateDetails->m_actualStateQdot[5] = mb->getBaseOmega()[2]; totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF } @@ -6421,11 +6432,11 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc { for (int d = 0; d < mb->getLink(l).m_posVarCount; d++) { - serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; + stateDetails->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d]; } for (int d = 0; d < mb->getLink(l).m_dofCount; d++) { - serverCmd.m_sendActualStateArgs.m_jointMotorForce[totalDegreeOfFreedomU] = 0; + stateDetails->m_jointMotorForce[totalDegreeOfFreedomU] = 0; if (mb->getLink(l).m_jointType == btMultibodyLink::eSpherical) { @@ -6434,7 +6445,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc { btScalar impulse = motor->getAppliedImpulse(d); btScalar force = impulse / m_data->m_physicsDeltaTime; - serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; + stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; } } else @@ -6446,19 +6457,19 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc if (motor && m_data->m_physicsDeltaTime > btScalar(0)) { btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime; - serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; + stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; } } } - serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; + stateDetails->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; } if (0 == mb->getLink(l).m_jointFeedback) { for (int d = 0; d < 6; d++) { - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + d] = 0; + stateDetails->m_jointReactionForces[l * 6 + d] = 0; } } else @@ -6466,16 +6477,16 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 0] = sensedForce[0]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 1] = sensedForce[1]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 2] = sensedForce[2]; + stateDetails->m_jointReactionForces[l * 6 + 0] = sensedForce[0]; + stateDetails->m_jointReactionForces[l * 6 + 1] = sensedForce[1]; + stateDetails->m_jointReactionForces[l * 6 + 2] = sensedForce[2]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 3] = sensedTorque[0]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 4] = sensedTorque[1]; - serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 5] = sensedTorque[2]; + stateDetails->m_jointReactionForces[l * 6 + 3] = sensedTorque[0]; + stateDetails->m_jointReactionForces[l * 6 + 4] = sensedTorque[1]; + stateDetails->m_jointReactionForces[l * 6 + 5] = sensedTorque[2]; } - serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; + stateDetails->m_jointMotorForce[l] = 0; if (supportsJointMotor(mb, l)) { @@ -6484,7 +6495,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc if (motor && m_data->m_physicsDeltaTime > btScalar(0)) { btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime; - serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = + stateDetails->m_jointMotorForce[l] = force; //if (force>0) //{ @@ -6498,13 +6509,13 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); - serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 0] = linkCOMOrigin.getX(); - serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 1] = linkCOMOrigin.getY(); - serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 2] = linkCOMOrigin.getZ(); - serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 3] = linkCOMRotation.x(); - serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 4] = linkCOMRotation.y(); - serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 5] = linkCOMRotation.z(); - serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 6] = linkCOMRotation.w(); + stateDetails->m_linkState[l * 7 + 0] = linkCOMOrigin.getX(); + stateDetails->m_linkState[l * 7 + 1] = linkCOMOrigin.getY(); + stateDetails->m_linkState[l * 7 + 2] = linkCOMOrigin.getZ(); + stateDetails->m_linkState[l * 7 + 3] = linkCOMRotation.x(); + stateDetails->m_linkState[l * 7 + 4] = linkCOMRotation.y(); + stateDetails->m_linkState[l * 7 + 5] = linkCOMRotation.z(); + stateDetails->m_linkState[l * 7 + 6] = linkCOMRotation.w(); btVector3 worldLinVel(0, 0, 0); btVector3 worldAngVel(0, 0, 0); @@ -6516,21 +6527,21 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc worldAngVel = linkRotMat * omega[l + 1]; } - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 0] = worldLinVel[0]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 1] = worldLinVel[1]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 2] = worldLinVel[2]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 3] = worldAngVel[0]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 4] = worldAngVel[1]; - serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 5] = worldAngVel[2]; + stateDetails->m_linkWorldVelocities[l * 6 + 0] = worldLinVel[0]; + stateDetails->m_linkWorldVelocities[l * 6 + 1] = worldLinVel[1]; + stateDetails->m_linkWorldVelocities[l * 6 + 2] = worldLinVel[2]; + stateDetails->m_linkWorldVelocities[l * 6 + 3] = worldAngVel[0]; + stateDetails->m_linkWorldVelocities[l * 6 + 4] = worldAngVel[1]; + stateDetails->m_linkWorldVelocities[l * 6 + 5] = worldAngVel[2]; - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 0] = linkLocalInertialOrigin.getX(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 1] = linkLocalInertialOrigin.getY(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 2] = linkLocalInertialOrigin.getZ(); + stateDetails->m_linkLocalInertialFrames[l * 7 + 0] = linkLocalInertialOrigin.getX(); + stateDetails->m_linkLocalInertialFrames[l * 7 + 1] = linkLocalInertialOrigin.getY(); + stateDetails->m_linkLocalInertialFrames[l * 7 + 2] = linkLocalInertialOrigin.getZ(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 3] = linkLocalInertialRotation.x(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 4] = linkLocalInertialRotation.y(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 5] = linkLocalInertialRotation.z(); - serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 6] = linkLocalInertialRotation.w(); + stateDetails->m_linkLocalInertialFrames[l * 7 + 3] = linkLocalInertialRotation.x(); + stateDetails->m_linkLocalInertialFrames[l * 7 + 4] = linkLocalInertialRotation.y(); + stateDetails->m_linkLocalInertialFrames[l * 7 + 5] = linkLocalInertialRotation.z(); + stateDetails->m_linkLocalInertialFrames[l * 7 + 6] = linkLocalInertialRotation.w(); } serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; @@ -6545,35 +6556,36 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc btRigidBody* rb = body->m_rigidBody; SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED; - + serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; serverCmd.m_sendActualStateArgs.m_numLinks = 0; + serverCmd.m_sendActualStateArgs.m_stateDetails = 0; int totalDegreeOfFreedomQ = 0; int totalDegreeOfFreedomU = 0; btTransform tr = rb->getWorldTransform(); //base position in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; + stateDetails->m_actualStateQ[0] = tr.getOrigin()[0]; + stateDetails->m_actualStateQ[1] = tr.getOrigin()[1]; + stateDetails->m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; - serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; + stateDetails->m_actualStateQ[3] = tr.getRotation()[0]; + stateDetails->m_actualStateQ[4] = tr.getRotation()[1]; + stateDetails->m_actualStateQ[5] = tr.getRotation()[2]; + stateDetails->m_actualStateQ[6] = tr.getRotation()[3]; totalDegreeOfFreedomQ += 7; //pos + quaternion //base linear velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; + stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0]; + stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1]; + stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2]; //base angular velocity (in world space, carthesian) - serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; - serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2]; + stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0]; + stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1]; + stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2]; totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; diff --git a/examples/SharedMemory/SharedMemoryCommandProcessor.cpp b/examples/SharedMemory/SharedMemoryCommandProcessor.cpp index d4b722af9..952be4128 100644 --- a/examples/SharedMemory/SharedMemoryCommandProcessor.cpp +++ b/examples/SharedMemory/SharedMemoryCommandProcessor.cpp @@ -17,7 +17,8 @@ struct SharedMemoryCommandProcessorInternalData bool m_waitingForServer; SharedMemoryStatus m_lastServerStatus; SharedMemoryBlock* m_testBlock1; - + SendActualStateSharedMemoryStorage m_cachedState; + SharedMemoryCommandProcessorInternalData() : m_sharedMemoryKey(SHARED_MEMORY_KEY), m_isConnected(false), @@ -160,6 +161,14 @@ bool SharedMemoryCommandProcessor::receiveStatus(struct SharedMemoryStatus& serv m_data->m_testBlock1->m_numProcessedServerCommands + 1); const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0]; + if (serverCmd.m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + SendActualStateSharedMemoryStorage* serverState = (SendActualStateSharedMemoryStorage*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; + m_data->m_cachedState = *serverState; + //ideally we provided a 'getCachedState' but that would require changing the API, so we store a pointer instead. + m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_stateDetails = &m_data->m_cachedState; + } + m_data->m_lastServerStatus = serverCmd; m_data->m_lastServerStatus.m_dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b16364295..8b4dfcb4c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -522,7 +522,12 @@ struct SendActualStateArgs int m_numDegreeOfFreedomU; double m_rootLocalInertialFrame[7]; + struct SendActualStateSharedMemoryStorage* m_stateDetails; +}; + +struct SendActualStateSharedMemoryStorage +{ //actual state is only written by the server, read-only access by client is expected double m_actualStateQ[MAX_DEGREE_OF_FREEDOM]; double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM]; diff --git a/examples/SharedMemory/grpc/ConvertGRPCBullet.cpp b/examples/SharedMemory/grpc/ConvertGRPCBullet.cpp index 6b9d99585..86acbe0d2 100644 --- a/examples/SharedMemory/grpc/ConvertGRPCBullet.cpp +++ b/examples/SharedMemory/grpc/ConvertGRPCBullet.cpp @@ -1260,13 +1260,15 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se serverStatus.m_sendActualStateArgs.m_numDegreeOfFreedomQ = numDegreeOfFreedomQ; serverStatus.m_sendActualStateArgs.m_numDegreeOfFreedomU = numDegreeOfFreedomU; + serverStatus.m_sendActualStateArgs.m_stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient; + for (int i = 0; i < numDegreeOfFreedomQ; i++) { - serverStatus.m_sendActualStateArgs.m_actualStateQ[i] = stat->actualstateq(i); + serverStatus.m_sendActualStateArgs.m_stateDetails->m_actualStateQ[i] = stat->actualstateq(i); } for (int i = 0; i < numDegreeOfFreedomU; i++) { - serverStatus.m_sendActualStateArgs.m_actualStateQdot[i] = stat->actualstateqdot(i); + serverStatus.m_sendActualStateArgs.m_stateDetails->m_actualStateQdot[i] = stat->actualstateqdot(i); } for (int i = 0; i < 7; i++) { @@ -1274,23 +1276,23 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se } for (int i = 0; i < numLinks * 6; i++) { - serverStatus.m_sendActualStateArgs.m_linkLocalInertialFrames[i] = stat->linklocalinertialframes(i); + serverStatus.m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[i] = stat->linklocalinertialframes(i); } for (int i = 0; i < numLinks * 6; i++) { - serverStatus.m_sendActualStateArgs.m_jointReactionForces[i] = stat->jointreactionforces(i); + serverStatus.m_sendActualStateArgs.m_stateDetails->m_jointReactionForces[i] = stat->jointreactionforces(i); } for (int i = 0; i < numLinks; i++) { - serverStatus.m_sendActualStateArgs.m_jointMotorForce[i] = stat->jointmotorforce(i); + serverStatus.m_sendActualStateArgs.m_stateDetails->m_jointMotorForce[i] = stat->jointmotorforce(i); } for (int i = 0; i < numLinks * 7; i++) { - serverStatus.m_sendActualStateArgs.m_linkState[i] = stat->linkstate(i); + serverStatus.m_sendActualStateArgs.m_stateDetails->m_linkState[i] = stat->linkstate(i); } for (int i = 0; i < numLinks * 6; i++) { - serverStatus.m_sendActualStateArgs.m_linkWorldVelocities[i] = stat->linkworldvelocities(i); + serverStatus.m_sendActualStateArgs.m_stateDetails->m_linkWorldVelocities[i] = stat->linkworldvelocities(i); } break;