From 822ff077c7fb66215b20a303336802fa0154aa56 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 5 Oct 2017 11:43:14 -0700 Subject: [PATCH] expose some parameters through pybullet.getPhysicsEngineParameters (C-API: b3InitRequestPhysicsParamCommand + b3GetStatusPhysicsSimulationParameters) --- .../ImportObjDemo/LoadMeshFromObj.cpp | 4 ++ .../Importers/ImportObjDemo/LoadMeshFromObj.h | 1 + examples/SharedMemory/PhysicsClientC_API.cpp | 36 ++++++++++--- examples/SharedMemory/PhysicsClientC_API.h | 7 ++- .../PhysicsClientSharedMemory.cpp | 5 +- examples/SharedMemory/PhysicsDirect.cpp | 4 ++ .../PhysicsServerCommandProcessor.cpp | 43 ++++++++++++--- examples/SharedMemory/SharedMemoryCommands.h | 23 +------- examples/SharedMemory/SharedMemoryPublic.h | 22 +++++++- examples/pybullet/pybullet.c | 53 +++++++++++++++++++ 10 files changed, 156 insertions(+), 42 deletions(-) diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp index a7cf426cd..ae7c13a1d 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp @@ -17,6 +17,10 @@ struct CachedObjResult static b3HashMap gCachedObjResults; static int gEnableFileCaching = 1; +int b3IsFileCachingEnabled() +{ + return gEnableFileCaching; +} void b3EnableFileCaching(int enable) { gEnableFileCaching = enable; diff --git a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h index 4d23eea8d..9414987f4 100644 --- a/examples/Importers/ImportObjDemo/LoadMeshFromObj.h +++ b/examples/Importers/ImportObjDemo/LoadMeshFromObj.h @@ -6,6 +6,7 @@ struct GLInstanceGraphicsShape; #include"../../ThirdPartyLibs/Wavefront/tiny_obj_loader.h" +int b3IsFileCachingEnabled(); void b3EnableFileCaching(int enable); std::string LoadFromCachedOrFromObj( diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index c8f0ed604..8a74f9769 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index a5723c589..54189dafa 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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 diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 31d0c96d8..050a9a257 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1225,7 +1225,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { } break; } - + case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED: + { + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 7369cd43d..94922ff0d 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -978,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"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 271450a0d..db744441f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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; @@ -1440,7 +1440,7 @@ struct PhysicsServerCommandProcessorInternalData b3PluginManager m_pluginManager; - bool m_allowRealTimeSimulation; + bool m_useRealTimeSimulation; b3VRControllerEvents m_vrControllerEvents; @@ -1524,7 +1524,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.), @@ -5690,6 +5690,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 +5727,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 @@ -8351,12 +8378,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) @@ -8461,7 +8488,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) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index edc2052d8..5a983647c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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 { @@ -971,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; @@ -1079,7 +1060,7 @@ struct SharedMemoryStatus struct SendMouseEvents m_sendMouseEvents; struct b3LoadTextureResultArgs m_loadTextureResultArguments; struct b3CustomCommandResultArgs m_customCommandResultArgs; - + struct b3PhysicsSimulationParameters m_simulationParameterResultArgs; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 6a1bf33ec..0d766fc64 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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 }; @@ -640,5 +641,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 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b3be00d45..302e4a5e7 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -853,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,¶ms); + + //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) { @@ -7429,6 +7479,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"},