Merge pull request #1358 from erwincoumans/master

pybullet.getConnectionInfo, store mass matrix in streaming part of shared memory etc.
This commit is contained in:
erwincoumans 2017-10-05 14:00:56 -07:00 committed by GitHub
commit b4af0e618a
22 changed files with 389 additions and 124 deletions

Binary file not shown.

View File

@ -55,7 +55,7 @@ public:
virtual bool mouseButtonCallback(int button, int state, float x, float y)=0;
virtual bool keyboardCallback(int key, int state)=0;
virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis) {}
virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis, float auxAnalogAxes[10]) {}
virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){}
virtual void vrHMDMoveCallback(int controllerId, float pos[4], float orientation[4]){}
virtual void vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orientation[4]){}

View File

@ -377,6 +377,13 @@ void OpenGLExampleBrowser::registerFileImporter(const char* extension, CommonExa
void OpenGLExampleBrowserVisualizerFlagCallback(int flag, bool enable)
{
if (flag == COV_ENABLE_Y_AXIS_UP)
{
//either Y = up or Z
int upAxis = enable? 1:2;
s_app->setUpAxis(upAxis);
}
if (flag == COV_ENABLE_RENDERING)
{
gEnableRenderLoop = (enable!=0);

View File

@ -30,19 +30,16 @@ static OpenGLExampleBrowser* sExampleBrowser=0;
static void cleanup(int signo)
{
if (interrupted) { // this is the second time, we're hanging somewhere
// if (example) {
// example->abort();
// }
if (!interrupted) { // this is the second time, we're hanging somewhere
b3Printf("Aborting and deleting SharedMemoryCommon object");
sleep(1);
delete sExampleBrowser;
sExampleBrowser = 0;
sleep(1);
sExampleBrowser = 0;
errx(EXIT_FAILURE, "aborted example on signal %d", signo);
} else
{
b3Printf("no action");
b3Printf("no action");
exit(EXIT_FAILURE);
}
interrupted = true;
warnx("caught signal %d", signo);

View File

@ -17,6 +17,10 @@ struct CachedObjResult
static b3HashMap<b3HashString, CachedObjResult> gCachedObjResults;
static int gEnableFileCaching = 1;
int b3IsFileCachingEnabled()
{
return gEnableFileCaching;
}
void b3EnableFileCaching(int enable)
{
gEnableFileCaching = enable;

View File

@ -6,6 +6,7 @@ struct GLInstanceGraphicsShape;
#include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h"
int b3IsFileCachingEnabled();
void b3EnableFileCaching(int enable);
std::string LoadFromCachedOrFromObj(

View File

@ -66,6 +66,8 @@ public:
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits) = 0;
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix) = 0;
virtual void setTimeOut(double timeOutInSeconds) = 0;
virtual double getTimeOut() const = 0;

View File

@ -325,6 +325,32 @@ B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHand
return -1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle,struct b3PhysicsSimulationParameters* params)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED);
if (status && status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED)
{
*params = status->m_simulationParameterResultArgs;
return 1;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@ -352,7 +378,7 @@ B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandH
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_allowRealTimeSimulation = (enableRealTimeSimulation!=0);
command->m_physSimParamArgs.m_useRealTimeSimulation = (enableRealTimeSimulation!=0);
command->m_updateFlags |= SIM_PARAM_UPDATE_REAL_TIME_SIMULATION;
return 0;
}
@ -395,15 +421,11 @@ B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryComman
command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_maxNumCmdPer1ms = maxNumCmdPer1ms;
command->m_updateFlags |= SIM_PARAM_MAX_CMD_PER_1MS;
//obsolete command
return 0;
}
B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching)
@ -3417,8 +3439,11 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy
}
B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
@ -3430,12 +3455,7 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle,
}
if (massMatrix)
{
int numElements = status->m_massMatrixResultArgs.m_dofCount * status->m_massMatrixResultArgs.m_dofCount;
for (int i = 0; i < numElements; i++)
{
massMatrix[i] = status->m_massMatrixResultArgs.m_massMatrix[i];
}
cl->getCachedMassMatrix(status->m_massMatrixResultArgs.m_dofCount, massMatrix);
}
return true;

View File

@ -263,14 +263,10 @@ B3_SHARED_API int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandH
B3_SHARED_API int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
B3_SHARED_API int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP);
B3_SHARED_API int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP);
B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
@ -279,6 +275,9 @@ B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle
B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle,struct b3PhysicsSimulationParameters* params);
//b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
//Use at own risk: magic things may or my not happen when calling this API
@ -323,10 +322,8 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
double* angularJacobian);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ);
B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle,
int* dofCount,
double* massMatrix);
///the mass matrix is stored in column-major layout of size dofCount*dofCount
B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix);
///compute the joint positions to move the end effector to a desired target using inverse kinematics

View File

@ -47,7 +47,7 @@ struct PhysicsClientSharedMemoryInternalData {
btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
btAlignedObjectArray<b3MouseEvent> m_cachedMouseEvents;
btAlignedObjectArray<double> m_cachedMassMatrix;
btAlignedObjectArray<b3RayHitInfo> m_raycastHits;
btAlignedObjectArray<int> m_bodyIdsRequestInfo;
@ -1210,6 +1210,25 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_CALCULATED_MASS_MATRIX_FAILED:
{
b3Warning("calculate mass matrix failed");
break;
}
case CMD_CALCULATED_MASS_MATRIX_COMPLETED:
{
double* matrixData = (double*)&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount);
for (int i=0;i<serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount;i++)
{
m_data->m_cachedMassMatrix[i] = matrixData[i];
}
break;
}
case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED:
{
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);
btAssert(0);
@ -1531,6 +1550,21 @@ void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation
}
void PhysicsClientSharedMemory::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
{
int sz = dofCountCheck*dofCountCheck;
if (sz == m_data->m_cachedMassMatrix.size())
{
for (int i=0;i<sz;i++)
{
massMatrix[i] = m_data->m_cachedMassMatrix[i];
}
}
}
void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();

View File

@ -76,6 +76,8 @@ public:
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;

View File

@ -40,7 +40,7 @@ struct PhysicsDirectInternalData
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
btAlignedObjectArray<double> m_cachedMassMatrix;
int m_cachedCameraPixelsWidth;
int m_cachedCameraPixelsHeight;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
@ -951,6 +951,21 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
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;i<serverCmd.m_massMatrixResultArgs.m_dofCount*serverCmd.m_massMatrixResultArgs.m_dofCount;i++)
{
m_data->m_cachedMassMatrix[i] = matrixData[i];
}
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
break;
@ -963,6 +978,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
break;
}
case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED:
{
break;
}
default:
{
//b3Warning("Unknown server status type");
@ -1220,6 +1239,18 @@ void PhysicsDirect::getCachedRaycastHits(struct b3RaycastInformation* raycastHit
raycastHits->m_rayHits = raycastHits->m_numRayHits? &m_data->m_raycastHits[0] : 0;
}
void PhysicsDirect::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
{
int sz = dofCountCheck*dofCountCheck;
if (sz == m_data->m_cachedMassMatrix.size())
{
for (int i=0;i<sz;i++)
{
massMatrix[i] = m_data->m_cachedMassMatrix[i];
}
}
}
void PhysicsDirect::setTimeOut(double timeOutInSeconds)
{
m_data->m_timeOutInSeconds = timeOutInSeconds;

View File

@ -99,6 +99,8 @@ public:
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();

View File

@ -205,6 +205,11 @@ void PhysicsLoopBack::getCachedRaycastHits(struct b3RaycastInformation* raycastH
return m_data->m_physicsClient->getCachedRaycastHits(raycastHits);
}
void PhysicsLoopBack::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
{
m_data->m_physicsClient->getCachedMassMatrix(dofCountCheck,massMatrix);
}
void PhysicsLoopBack::setTimeOut(double timeOutInSeconds)
{
m_data->m_physicsClient->setTimeOut(timeOutInSeconds);

View File

@ -80,6 +80,8 @@ public:
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
};

View File

@ -360,7 +360,7 @@ struct CommandLogger
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
fwrite((const char*)&command.m_updateFlags,sizeof(int), 1,m_file);
fwrite((const char*)&command.m_physSimParamArgs, sizeof(SendPhysicsSimulationParameters), 1,m_file);
fwrite((const char*)&command.m_physSimParamArgs, sizeof(b3PhysicsSimulationParameters), 1,m_file);
break;
}
case CMD_REQUEST_CONTACT_POINT_INFORMATION:
@ -564,7 +564,7 @@ struct CommandLogPlayback
cmd->m_physSimParamArgs = unused.m_physSimParamArgs;
#else
fread(&cmd->m_updateFlags,sizeof(int),1,m_file);
fread(&cmd->m_physSimParamArgs ,sizeof(SendPhysicsSimulationParameters),1,m_file);
fread(&cmd->m_physSimParamArgs ,sizeof(b3PhysicsSimulationParameters),1,m_file);
#endif
result = true;
@ -889,8 +889,18 @@ struct b3VRControllerEvents
if (vrEvents[i].m_numMoveEvents)
{
m_vrEvents[controlledId].m_analogAxis = vrEvents[i].m_analogAxis;
for (int a=0;a<10;a++)
{
m_vrEvents[controlledId].m_auxAnalogAxis[a] = vrEvents[i].m_auxAnalogAxis[a];
}
} else
{
m_vrEvents[controlledId].m_analogAxis = 0;
for (int a=0;a<10;a++)
{
m_vrEvents[controlledId].m_auxAnalogAxis[a] = 0;
}
}
if (vrEvents[i].m_numMoveEvents+vrEvents[i].m_numButtonEvents)
{
m_vrEvents[controlledId].m_controllerId = vrEvents[i].m_controllerId;
@ -1440,7 +1450,7 @@ struct PhysicsServerCommandProcessorInternalData
b3PluginManager m_pluginManager;
bool m_allowRealTimeSimulation;
bool m_useRealTimeSimulation;
b3VRControllerEvents m_vrControllerEvents;
@ -1524,7 +1534,7 @@ struct PhysicsServerCommandProcessorInternalData
PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
:m_pluginManager(proc),
m_allowRealTimeSimulation(false),
m_useRealTimeSimulation(false),
m_commandLogger(0),
m_logPlayback(0),
m_physicsDeltaTime(1./240.),
@ -3003,7 +3013,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes )
{
// BT_PROFILE("processCommand");
bool hasStatus = false;
{
@ -5690,6 +5700,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
break;
}
case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS:
{
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED;
serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = m_data->m_broadphaseCollisionFilterCallback->m_filterMode;
serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = gContactBreakingThreshold;
serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = m_data->m_dynamicsWorld->getSolverInfo().m_erp;
serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = b3IsFileCachingEnabled();
serverCmd.m_simulationParameterResultArgs.m_frictionERP = m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP;
btVector3 grav = m_data->m_dynamicsWorld->getGravity();
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav[0];
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav[1];
serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav[2];
serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = gInternalSimFlags;
serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps;
serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations;
serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold;
serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold;
serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = m_data->m_useRealTimeSimulation;
serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse;
hasStatus = true;
break;
}
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");
@ -5700,7 +5737,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION)
{
m_data->m_allowRealTimeSimulation = clientCmd.m_physSimParamArgs.m_allowRealTimeSimulation;
m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0);
}
//see
@ -6722,10 +6759,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
BT_PROFILE("CMD_CALCULATE_MASS_MATRIX");
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
@ -6744,20 +6781,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs;
// Fill in the result into the shared memory.
for (int i = 0; i < (totDofs); ++i)
{
for (int j = 0; j < (totDofs); ++j)
{
int element = (totDofs) * i + j;
serverCmd.m_massMatrixResultArgs.m_massMatrix[element] = massMatrix(i,j);
}
}
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
}
else
{
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED;
double* sharedBuf = (double*)bufferServerToClient;
int sizeInBytes = totDofs*totDofs*sizeof(double);
if (sizeInBytes < bufferSizeInBytes)
{
for (int i = 0; i < (totDofs); ++i)
{
for (int j = 0; j < (totDofs); ++j)
{
int element = (totDofs) * i + j;
sharedBuf[element] = massMatrix(i,j);
}
}
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
}
}
}
}
@ -8348,12 +8388,12 @@ double gSubStep = 0.f;
void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTimeSim)
{
m_data->m_allowRealTimeSimulation = enableRealTimeSim;
m_data->m_useRealTimeSimulation = enableRealTimeSim;
}
bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const
{
return m_data->m_allowRealTimeSimulation;
return m_data->m_useRealTimeSimulation;
}
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)
@ -8458,7 +8498,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const
}
}
if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
if ((m_data->m_useRealTimeSimulation) && m_data->m_guiHelper)
{

View File

@ -1330,7 +1330,7 @@ public:
btVector3 getRayTo(int x,int y);
virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]);
virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis);
virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis, float auxAnalogAxes[10]);
virtual void vrHMDMoveCallback(int controllerId, float pos[4], float orientation[4]);
virtual void vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orientation[4]);
@ -2874,7 +2874,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
}
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis, float auxAnalogAxes[10])
{
if (controllerId < 0 || controllerId >= MAX_VR_CONTROLLERS)
@ -2927,6 +2927,11 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++;
m_args[0].m_vrControllerEvents[controllerId].m_analogAxis = analogAxis;
for (int i=0;i<10;i++)
{
m_args[0].m_vrControllerEvents[controllerId].m_auxAnalogAxis[i] = auxAnalogAxes[i];
}
m_args[0].m_csGUI->unlock();
}

View File

@ -428,25 +428,6 @@ enum EnumSimParamInternalSimFlags
///Controlling a robot involves sending the desired state to its joint motor controllers.
///The control mode determines the state variables used for motor control.
struct SendPhysicsSimulationParameters
{
double m_deltaTime;
double m_gravityAcceleration[3];
int m_numSimulationSubSteps;
int m_numSolverIterations;
bool m_allowRealTimeSimulation;
int m_useSplitImpulse;
double m_splitImpulsePenetrationThreshold;
double m_contactBreakingThreshold;
int m_maxNumCmdPer1ms;
int m_internalSimFlags;
double m_defaultContactERP;
int m_collisionFilterMode;
int m_enableFileCaching;
double m_restitutionVelocityThreshold;
double m_defaultNonContactERP;
double m_frictionERP;
};
struct LoadBunnyArgs
{
@ -650,7 +631,6 @@ struct CalculateMassMatrixArgs
struct CalculateMassMatrixResultArgs
{
int m_dofCount;
double m_massMatrix[MAX_DEGREE_OF_FREEDOM * MAX_DEGREE_OF_FREEDOM];
};
enum EnumCalculateInverseKinematicsFlags
@ -972,7 +952,7 @@ struct SharedMemoryCommand
struct ChangeDynamicsInfoArgs m_changeDynamicsInfoArgs;
struct GetDynamicsInfoArgs m_getDynamicsInfoArgs;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct b3PhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments;
struct SendDesiredStateArgs m_sendDesiredStateCommandArgument;
struct RequestActualStateArgs m_requestActualStateInformationCommandArgument;
@ -1080,7 +1060,7 @@ struct SharedMemoryStatus
struct SendMouseEvents m_sendMouseEvents;
struct b3LoadTextureResultArgs m_loadTextureResultArguments;
struct b3CustomCommandResultArgs m_customCommandResultArgs;
struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
};
};

View File

@ -5,7 +5,7 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201709260
#define SHARED_MEMORY_MAGIC_NUMBER 201710050
//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
//#define SHARED_MEMORY_MAGIC_NUMBER 201707140
//#define SHARED_MEMORY_MAGIC_NUMBER 201706015
@ -75,6 +75,7 @@ enum EnumSharedMemoryClientCommand
CMD_CHANGE_TEXTURE,
CMD_SET_ADDITIONAL_SEARCH_PATH,
CMD_CUSTOM_COMMAND,
CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@ -174,7 +175,7 @@ enum EnumSharedMemoryServerStatus
CMD_CHANGE_TEXTURE_COMMAND_FAILED,
CMD_CUSTOM_COMMAND_COMPLETED,
CMD_CUSTOM_COMMAND_FAILED,
CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@ -338,6 +339,7 @@ enum b3VREventType
VR_GENERIC_TRACKER_MOVE_EVENT=8,
};
#define MAX_VR_ANALOG_AXIS 5
#define MAX_VR_BUTTONS 64
#define MAX_VR_CONTROLLERS 8
@ -379,7 +381,7 @@ struct b3VRControllerEvent
float m_orn[4];//valid for VR_CONTROLLER_MOVE_EVENT and VR_CONTROLLER_BUTTON_EVENT
float m_analogAxis;//valid if VR_CONTROLLER_MOVE_EVENT
float m_auxAnalogAxis[MAX_VR_ANALOG_AXIS*2];//store x,y per axis, only valid if VR_CONTROLLER_MOVE_EVENT
int m_buttons[MAX_VR_BUTTONS];//valid if VR_CONTROLLER_BUTTON_EVENT, see b3VRButtonInfo
};
@ -575,6 +577,7 @@ enum b3ConfigureDebugVisualizerEnum
COV_ENABLE_SYNC_RENDERING_INTERNAL,
COV_ENABLE_KEYBOARD_SHORTCUTS,
COV_ENABLE_MOUSE_PICKING,
COV_ENABLE_Y_AXIS_UP,
};
enum b3AddUserDebugItemEnum
@ -639,5 +642,24 @@ struct b3PluginArguments
double m_floats[B3_MAX_PLUGIN_ARG_SIZE];
};
struct b3PhysicsSimulationParameters
{
double m_deltaTime;
double m_gravityAcceleration[3];
int m_numSimulationSubSteps;
int m_numSolverIterations;
int m_useRealTimeSimulation;
int m_useSplitImpulse;
double m_splitImpulsePenetrationThreshold;
double m_contactBreakingThreshold;
int m_internalSimFlags;
double m_defaultContactERP;
int m_collisionFilterMode;
int m_enableFileCaching;
double m_restitutionVelocityThreshold;
double m_defaultNonContactERP;
double m_frictionERP;
};
#endif//SHARED_MEMORY_PUBLIC_H

View File

@ -376,10 +376,16 @@ void MyKeyboardCallback(int key, int state)
extern bool useShadowMap;
static bool gEnableVRRenderControllers=true;
static bool gEnableVRRendering = true;
static int gUpAxis = 2;
void VRPhysicsServerVisualizerFlagCallback(int flag, bool enable)
{
if (flag == COV_ENABLE_Y_AXIS_UP)
{
//either Y = up or Z
gUpAxis = enable? 1:2;
}
if (flag == COV_ENABLE_SHADOWS)
{
useShadowMap = enable;
@ -808,7 +814,15 @@ bool CMainApplication::HandleInput()
for (int button = 0; button < vr::k_EButton_Max; button++)
{
uint64_t trigger = vr::ButtonMaskFromId((vr::EVRButtonId)button);
btAssert(vr::k_unControllerStateAxisCount>=5);
float allAxis[10];//store x,y times 5 controllers
int index=0;
for (int i=0;i<5;i++)
{
allAxis[index++]=state.rAxis[i].x;
allAxis[index++]=state.rAxis[i].y;
}
bool isTrigger = (state.ulButtonPressed&trigger) != 0;
if (isTrigger)
{
@ -818,31 +832,15 @@ bool CMainApplication::HandleInput()
if ((sPrevStates[unDevice].ulButtonPressed&trigger)==0)
{
// printf("Device PRESSED: %d, button %d\n", unDevice, button);
if (button==2)
{
//glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync
///so it can (and likely will) cause crashes
///add a special debug drawer that deals with this
//gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints+
//btIDebugDraw::DBG_DrawConstraintLimits+
//btIDebugDraw::DBG_DrawConstraints
//;
//gDebugDrawFlags = btIDebugDraw::DBG_DrawFrames;
}
sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn);
}
else
{
// printf("Device MOVED: %d\n", unDevice);
sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x, allAxis);
}
}
else
@ -865,7 +863,7 @@ bool CMainApplication::HandleInput()
} else
{
sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x,allAxis);
}
}
}
@ -890,6 +888,7 @@ void CMainApplication::RunMainLoop()
while ( !bQuit && !m_app->m_window->requestedExit())
{
this->m_app->setUpAxis(gUpAxis);
b3ChromeUtilsEnableProfiling();
if (gEnableVRRendering)
{

View File

@ -8,7 +8,9 @@ import pybullet as p
CONTROLLER_ID = 0
POSITION=1
ORIENTATION=2
NUM_MOVE_EVENTS=5
BUTTONS=6
ANALOG_AXIS=8
#assume that the VR physics server is already started before
c = p.connect(p.SHARED_MEMORY)
@ -20,7 +22,7 @@ p.setInternalSimFlags(0)#don't load default robot assets etc
p.resetSimulation()
p.loadURDF("plane.urdf")
prevPosition=[None]*p.VR_MAX_CONTROLLERS
prevPosition=[[0,0,0]]*p.VR_MAX_CONTROLLERS
colors=[0.,0.5,0.5]*p.VR_MAX_CONTROLLERS
widths = [3]*p.VR_MAX_CONTROLLERS
@ -32,10 +34,27 @@ colors[3] = [0,0,0.5]
colors[4] = [0.5,0.5,0.]
colors[5] = [.5,.5,.5]
while True:
controllerId = -1
pt=[0,0,0]
print("waiting for VR controller trigger")
while (controllerId<0):
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
if (e[BUTTONS][32]==p.VR_BUTTON_IS_DOWN):
controllerId = e[CONTROLLER_ID]
print("Using controllerId="+str(controllerId))
while True:
events = p.getVREvents(allAnalogAxes=1)
for e in (events):
if (e[CONTROLLER_ID]==controllerId ):
for a in range(10):
print("analog axis"+str(a)+"="+str(e[8][a]))
if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED):
prevPosition[e[CONTROLLER_ID]] = e[POSITION]
if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED):
@ -51,7 +70,10 @@ while True:
pt = prevPosition[e[CONTROLLER_ID]]
#print(prevPosition[e[0]])
#print(e[1])
print("e[POSITION]")
print(e[POSITION])
print("pt")
print(pt)
diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]]
lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
ptDistThreshold = 0.01

View File

@ -503,6 +503,35 @@ void b3pybulletExitFunc(void)
}
static PyObject* pybullet_getConnectionInfo(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
int isConnected=0;
int method=0;
PyObject* pylist = 0;
PyObject* val = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm != 0)
{
if (b3CanSubmitCommand(sm))
{
isConnected = 1;
method = sPhysicsClientsGUI[physicsClientId];
}
}
val = Py_BuildValue("{s:i,s:i}","isConnected", isConnected, "connectionMethod", method);
return val;
}
static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args, PyObject* keywds)
{
const char* worldFileName = "";
@ -824,6 +853,56 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
return NULL;
}
static PyObject* pybullet_getPhysicsEngineParameters(PyObject* self, PyObject* args, PyObject* keywds)
{
b3PhysicsClientHandle sm = 0;
PyObject* val=0;
int physicsClientId = 0;
static char* kwlist[] = {"physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
b3SharedMemoryCommandHandle command = b3InitRequestPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
struct b3PhysicsSimulationParameters params;
int statusType;
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
if (statusType!=CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED)
{
PyErr_SetString(SpamError, "Couldn't get physics simulation parameters.");
return NULL;
}
b3GetStatusPhysicsSimulationParameters(statusHandle,&params);
//for now, return a subset, expose more/all on request
val = Py_BuildValue("{s:d,s:i,s:i,s:i,s:d,s:d,s:d}",
"fixedTimeStep", params.m_deltaTime,
"numSubSteps", params.m_numSimulationSubSteps,
"numSolverIterations", params.m_numSolverIterations,
"useRealTimeSimulation", params.m_useRealTimeSimulation,
"gravityAccelerationX", params.m_gravityAcceleration[0],
"gravityAccelerationY", params.m_gravityAcceleration[1],
"gravityAccelerationZ", params.m_gravityAcceleration[2]
);
return val;
}
//"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP",
//val = Py_BuildValue("{s:i,s:i}","isConnected", isConnected, "connectionMethod", method);
}
static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds)
{
@ -918,19 +997,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
{
b3PhysicsParamSetDefaultFrictionERP(command,frictionERP);
}
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
#if 0
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx, double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
#endif
Py_INCREF(Py_None);
return Py_None;
@ -4207,9 +4277,10 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
int statusType;
int deviceTypeFilter = VR_DEVICE_CONTROLLER;
int physicsClientId = 0;
int allAnalogAxes = 0;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"deviceTypeFilter", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist, &deviceTypeFilter, &physicsClientId))
static char* kwlist[] = {"deviceTypeFilter", "allAnalogAxes", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iii", kwlist, &deviceTypeFilter, &allAnalogAxes, &physicsClientId))
{
return NULL;
}
@ -4237,7 +4308,8 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
vrEventsObj = PyTuple_New(vrEvents.m_numControllerEvents);
for (i = 0; i < vrEvents.m_numControllerEvents; i++)
{
PyObject* vrEventObj = PyTuple_New(8);
int numFields = allAnalogAxes? 9 : 8;
PyObject* vrEventObj = PyTuple_New(numFields);
PyTuple_SetItem(vrEventObj, 0, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_controllerId));
{
@ -4270,6 +4342,19 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject*
PyTuple_SetItem(vrEventObj, 6, buttonsObj);
}
PyTuple_SetItem(vrEventObj, 7, PyInt_FromLong(vrEvents.m_controllerEvents[i].m_deviceType));
if (allAnalogAxes)
{
PyObject* buttonsObj = PyTuple_New(MAX_VR_ANALOG_AXIS*2);
int b;
for (b = 0; b < MAX_VR_ANALOG_AXIS*2; b++)
{
PyObject* axisVal = PyFloat_FromDouble(vrEvents.m_controllerEvents[i].m_auxAnalogAxis[b]);
PyTuple_SetItem(buttonsObj, b, axisVal);
}
PyTuple_SetItem(vrEventObj, 8, buttonsObj);
}
PyTuple_SetItem(vrEventsObj, i, vrEventObj);
}
return vrEventsObj;
@ -7314,13 +7399,14 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py
if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED)
{
int dofCount;
b3GetStatusMassMatrix(statusHandle, &dofCount, NULL);
b3GetStatusMassMatrix(sm, statusHandle, &dofCount, NULL);
if (dofCount)
{
pyResultList = PyTuple_New(dofCount);
int byteSizeDofCount = sizeof(double) * dofCount;
pyResultList = PyTuple_New(dofCount);
massMatrix = (double*)malloc(dofCount * byteSizeDofCount);
b3GetStatusMassMatrix(statusHandle, NULL, massMatrix);
b3GetStatusMassMatrix(sm, statusHandle, NULL, massMatrix);
if (massMatrix)
{
int r;
@ -7372,6 +7458,10 @@ static PyMethodDef SpamMethods[] = {
"disconnect(physicsClientId=0)\n"
"Disconnect from the physics server."},
{"getConnectionInfo", (PyCFunction)pybullet_getConnectionInfo, METH_VARARGS | METH_KEYWORDS,
"getConnectionInfo(physicsClientId=0)\n"
"Return if a given client id is connected, and using what method."},
{"resetSimulation", (PyCFunction)pybullet_resetSimulation, METH_VARARGS | METH_KEYWORDS,
"resetSimulation(physicsClientId=0)\n"
"Reset the simulation: remove all objects and start from an empty world."},
@ -7404,6 +7494,9 @@ static PyMethodDef SpamMethods[] = {
{"setPhysicsEngineParameter", (PyCFunction)pybullet_setPhysicsEngineParameter, METH_VARARGS | METH_KEYWORDS,
"Set some internal physics engine parameter, such as cfm or erp etc."},
{"getPhysicsEngineParameters", (PyCFunction)pybullet_getPhysicsEngineParameters, METH_VARARGS | METH_KEYWORDS,
"Get the current values of internal physics engine parameters"},
{"setInternalSimFlags", (PyCFunction)pybullet_setInternalSimFlags, METH_VARARGS | METH_KEYWORDS,
"This is for experimental purposes, use at own risk, magic may or not happen"},