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

reduce size of SharedMemoryStatus by moving state details into shared memory streaming block.

Browse Source
This commit is contained in:
erwincoumans
2019-03-06 23:27:59 -08:00
parent 4c37558805
commit 0af0f193ee
13 changed files with 330 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

147
examples/RobotSimulator/MinitaurSimulatorExample.cpp Normal file
View File

@@ -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 <string>
#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);
}

22
examples/RobotSimulator/MinitaurSimulatorExample.h Normal file
View File

@@ -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

4
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -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)

3
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -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);

3
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@@ -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);

49
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;
}

22
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -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);

5
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -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:

3
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -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);

134
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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;
@@ -6548,32 +6559,33 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
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;

9
examples/SharedMemory/SharedMemoryCommandProcessor.cpp
View File

@@ -17,6 +17,7 @@ struct SharedMemoryCommandProcessorInternalData
bool m_waitingForServer;
SharedMemoryStatus m_lastServerStatus;
SharedMemoryBlock* m_testBlock1;
SendActualStateSharedMemoryStorage m_cachedState;
SharedMemoryCommandProcessorInternalData()
: m_sharedMemoryKey(SHARED_MEMORY_KEY),
@@ -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;

5
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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];

16
examples/SharedMemory/grpc/ConvertGRPCBullet.cpp
View File

@@ -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;