From ed4515ae17cbc45289052d183b3d8920c2835ebf Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 19 Jun 2019 09:45:29 -0700 Subject: [PATCH] for the GraphicsServer, expose a sync transform interval: only synchronize the transform once the stepSimulation exceeds this time interval. (for example, run the simulation at 1kHz but sync the graphics transforms to remove graphics server at 30Hz) --- examples/SharedMemory/PhysicsClientC_API.cpp | 13 ++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 174 ++++++++++-------- examples/SharedMemory/SharedMemoryCommands.h | 2 + .../examples/configureDebugVisualizer.py | 1 + examples/pybullet/pybullet.c | 11 +- 6 files changed, 120 insertions(+), 82 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 4ad76abb4..24d690713 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -5501,6 +5501,19 @@ B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(b3SharedMemo } +B3_SHARED_API void b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(b3SharedMemoryCommandHandle commandHandle, double remoteSyncTransformInterval) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER); + if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER) + { + command->m_updateFlags |= COV_SET_REMOTE_SYNC_TRANSFORM_INTERVAL; + command->m_configureOpenGLVisualizerArguments.m_remoteSyncTransformInterval = remoteSyncTransformInterval; + } +} + + B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2ddac17b3..406b949ef 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -210,6 +210,7 @@ extern "C" B3_SHARED_API void b3ConfigureOpenGLVisualizerSetLightPosition(b3SharedMemoryCommandHandle commandHandle, const float lightPosition[3]); B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapResolution(b3SharedMemoryCommandHandle commandHandle, int shadowMapResolution); B3_SHARED_API void b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(b3SharedMemoryCommandHandle commandHandle, int shadowMapWorldSize); + B3_SHARED_API void b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(b3SharedMemoryCommandHandle commandHandle, double remoteSyncTransformInterval); B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[/*3*/]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 690c25798..33c47e493 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1595,7 +1595,7 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_physicsDeltaTime; btScalar m_numSimulationSubSteps; - btScalar m_simulationTimestamp; + btScalar m_simulationTimestamp; btAlignedObjectArray m_multiBodyJointFeedbacks; b3HashMap m_inverseDynamicsBodies; b3HashMap m_inverseKinematicsHelpers; @@ -1672,6 +1672,9 @@ struct PhysicsServerCommandProcessorInternalData b3ThreadPool* m_threadPool; btScalar m_defaultCollisionMargin; + double m_remoteSyncTransformTime; + double m_remoteSyncTransformInterval; + PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc) : m_pluginManager(proc), m_useRealTimeSimulation(false), @@ -1681,7 +1684,7 @@ struct PhysicsServerCommandProcessorInternalData m_logPlaybackUid(-1), m_physicsDeltaTime(1. / 240.), m_numSimulationSubSteps(0), - m_simulationTimestamp(0), + m_simulationTimestamp(0), m_userConstraintUIDGenerator(1), m_broadphaseCollisionFilterCallback(0), m_pairCache(0), @@ -1705,72 +1708,72 @@ struct PhysicsServerCommandProcessorInternalData m_collisionFilterPlugin(-1), m_grpcPlugin(-1), m_threadPool(0), - m_defaultCollisionMargin(0.001) + m_defaultCollisionMargin(0.001), + m_remoteSyncTransformTime(1. / 30.), + m_remoteSyncTransformInterval(1. / 30.) { { //register static plugins: #ifdef STATIC_LINK_VR_PLUGIN - b3PluginFunctions funcs(initPlugin_vrSyncPlugin,exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin); + b3PluginFunctions funcs(initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin); funcs.m_preTickFunc = preTickPluginCallback_vrSyncPlugin; m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", funcs); #endif //STATIC_LINK_VR_PLUGIN } #ifndef SKIP_STATIC_PD_CONTROL_PLUGIN - { - //int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin) - b3PluginFunctions funcs(initPlugin_pdControlPlugin,exitPlugin_pdControlPlugin,executePluginCommand_pdControlPlugin); - funcs.m_preTickFunc = preTickPluginCallback_pdControlPlugin; - m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", funcs); - } + { + //int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc, PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc, PFN_GET_RENDER_INTERFACE getRendererFunc, PFN_TICK processClientCommandsFunc, PFN_GET_COLLISION_INTERFACE getCollisionFunc, bool initPlugin) + b3PluginFunctions funcs(initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin); + funcs.m_preTickFunc = preTickPluginCallback_pdControlPlugin; + m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", funcs); + } #endif //SKIP_STATIC_PD_CONTROL_PLUGIN #ifndef SKIP_COLLISION_FILTER_PLUGIN - { - b3PluginFunctions funcs(initPlugin_collisionFilterPlugin,exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin); - funcs.m_getCollisionFunc = getCollisionInterface_collisionFilterPlugin; - m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", funcs ); - m_pluginManager.selectCollisionPlugin(m_collisionFilterPlugin); - } + { + b3PluginFunctions funcs(initPlugin_collisionFilterPlugin, exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin); + funcs.m_getCollisionFunc = getCollisionInterface_collisionFilterPlugin; + m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", funcs); + m_pluginManager.selectCollisionPlugin(m_collisionFilterPlugin); + } #endif #ifdef ENABLE_STATIC_GRPC_PLUGIN - { - b3PluginFunctions funcs(initPlugin_grpcPlugin, exitPlugin_grpcPlugin, executePluginCommand_grpcPlugin); - funcs.m_processClientCommandsFunc = processClientCommands_grpcPlugin; - m_grpcPlugin = m_pluginManager.registerStaticLinkedPlugin("grpcPlugin", funcs); - } + { + b3PluginFunctions funcs(initPlugin_grpcPlugin, exitPlugin_grpcPlugin, executePluginCommand_grpcPlugin); + funcs.m_processClientCommandsFunc = processClientCommands_grpcPlugin; + m_grpcPlugin = m_pluginManager.registerStaticLinkedPlugin("grpcPlugin", funcs); + } #endif //ENABLE_STATIC_GRPC_PLUGIN #ifdef STATIC_EGLRENDERER_PLUGIN - { - bool initPlugin = false; - b3PluginFunctions funcs(initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin); - funcs.m_getRendererFunc = getRenderInterface_eglRendererPlugin; - int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", funcs, initPlugin); - m_pluginManager.selectPluginRenderer(renderPluginId); - } + { + bool initPlugin = false; + b3PluginFunctions funcs(initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin); + funcs.m_getRendererFunc = getRenderInterface_eglRendererPlugin; + int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", funcs, initPlugin); + m_pluginManager.selectPluginRenderer(renderPluginId); + } #endif //STATIC_EGLRENDERER_PLUGIN #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN - { - b3PluginFunctions funcs(initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin); - funcs.m_getRendererFunc=getRenderInterface_tinyRendererPlugin; - int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", funcs); - m_pluginManager.selectPluginRenderer(renderPluginId); - } + { + b3PluginFunctions funcs(initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin); + funcs.m_getRendererFunc = getRenderInterface_tinyRendererPlugin; + int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", funcs); + m_pluginManager.selectPluginRenderer(renderPluginId); + } #endif #ifdef B3_ENABLE_FILEIO_PLUGIN - { - b3PluginFunctions funcs(initPlugin_fileIOPlugin, exitPlugin_fileIOPlugin, executePluginCommand_fileIOPlugin); - funcs.m_fileIoFunc = getFileIOFunc_fileIOPlugin; - int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("fileIOPlugin", funcs); - m_pluginManager.selectFileIOPlugin(renderPluginId); - } + { + b3PluginFunctions funcs(initPlugin_fileIOPlugin, exitPlugin_fileIOPlugin, executePluginCommand_fileIOPlugin); + funcs.m_fileIoFunc = getFileIOFunc_fileIOPlugin; + int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("fileIOPlugin", funcs); + m_pluginManager.selectFileIOPlugin(renderPluginId); + } #endif - - m_vrControllerEvents.init(); m_bodyHandles.exitHandles(); @@ -1812,7 +1815,7 @@ struct PhysicsServerCommandProcessorInternalData Eigen::VectorXd pose, vel; pose.resize(7 + multiBody->getNumPosVars()); - vel.resize(7 + multiBody->getNumPosVars()); //?? + vel.resize(7 + multiBody->getNumPosVars()); //?? btTransform tr = multiBody->getBaseWorldTransform(); int dofsrc = 0; @@ -1868,49 +1871,48 @@ struct PhysicsServerCommandProcessorInternalData int dof = 7; int veldof = 7; - for (int l = 0; l < multiBody->getNumLinks(); l++) { switch (multiBody->getLink(l).m_jointType) { - case btMultibodyLink::eRevolute: - case btMultibodyLink::ePrismatic: - { - pose[dof++] = jointPositionsQ[dofsrc++]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - break; - } - case btMultibodyLink::eSpherical: - { - double quatXYZW[4]; - quatXYZW[0] = jointPositionsQ[dofsrc++]; - quatXYZW[1] = jointPositionsQ[dofsrc++]; - quatXYZW[2] = jointPositionsQ[dofsrc++]; - quatXYZW[3] = jointPositionsQ[dofsrc++]; + case btMultibodyLink::eRevolute: + case btMultibodyLink::ePrismatic: + { + pose[dof++] = jointPositionsQ[dofsrc++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + break; + } + case btMultibodyLink::eSpherical: + { + double quatXYZW[4]; + quatXYZW[0] = jointPositionsQ[dofsrc++]; + quatXYZW[1] = jointPositionsQ[dofsrc++]; + quatXYZW[2] = jointPositionsQ[dofsrc++]; + quatXYZW[3] = jointPositionsQ[dofsrc++]; - pose[dof++] = quatXYZW[3]; - pose[dof++] = quatXYZW[0]; - pose[dof++] = quatXYZW[1]; - pose[dof++] = quatXYZW[2]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - break; - } - case btMultibodyLink::eFixed: - { - break; - } - default: - { - assert(0); - } + pose[dof++] = quatXYZW[3]; + pose[dof++] = quatXYZW[0]; + pose[dof++] = quatXYZW[1]; + pose[dof++] = quatXYZW[2]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + break; + } + case btMultibodyLink::eFixed: + { + break; + } + default: + { + assert(0); + } } } btVector3 gravOrg = m_dynamicsWorld->getGravity(); - tVector grav (gravOrg[0], gravOrg[1], gravOrg[2], 0); + tVector grav(gravOrg[0], gravOrg[1], gravOrg[2], 0); rbdModel->SetGravity(grav); rbdModel->Update(pose, vel); @@ -7763,7 +7765,13 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S } serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; - syncPhysicsToGraphics2(); + m_data->m_remoteSyncTransformTime += deltaTimeScaled; + if (m_data->m_remoteSyncTransformTime >= m_data->m_remoteSyncTransformInterval) + { + m_data->m_remoteSyncTransformTime = 0; + syncPhysicsToGraphics2(); + } + return hasStatus; } @@ -9074,6 +9082,13 @@ bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(cons m_data->m_guiHelper->getRenderInterface()->setShadowMapWorldSize(worldSize); } } + + if (clientCmd.m_updateFlags & COV_SET_REMOTE_SYNC_TRANSFORM_INTERVAL) + { + m_data->m_remoteSyncTransformInterval = clientCmd.m_configureOpenGLVisualizerArguments.m_remoteSyncTransformInterval; + } + + return hasStatus; } @@ -12047,7 +12062,8 @@ void PhysicsServerCommandProcessor::addTransformChangedNotifications() void PhysicsServerCommandProcessor::resetSimulation() { //clean up all data - + m_data->m_remoteSyncTransformTime = m_data->m_remoteSyncTransformInterval; + m_data->m_simulationTimestamp = 0; m_data->m_cachedVUrdfisualShapes.clear(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 06ca9ab81..ed0bfaca7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -908,6 +908,7 @@ enum InternalOpenGLVisualizerUpdateFlags COV_SET_LIGHT_POSITION = 4, COV_SET_SHADOWMAP_RESOLUTION = 8, COV_SET_SHADOWMAP_WORLD_SIZE = 16, + COV_SET_REMOTE_SYNC_TRANSFORM_INTERVAL = 32, }; struct ConfigureOpenGLVisualizerRequest @@ -919,6 +920,7 @@ struct ConfigureOpenGLVisualizerRequest double m_lightPosition[3]; int m_shadowMapResolution; int m_shadowMapWorldSize; + double m_remoteSyncTransformInterval; int m_setFlag; int m_setEnabled; }; diff --git a/examples/pybullet/examples/configureDebugVisualizer.py b/examples/pybullet/examples/configureDebugVisualizer.py index e307504d3..e513161b9 100644 --- a/examples/pybullet/examples/configureDebugVisualizer.py +++ b/examples/pybullet/examples/configureDebugVisualizer.py @@ -12,6 +12,7 @@ radius=5 t = 0 p.configureDebugVisualizer(shadowMapWorldSize=5) p.configureDebugVisualizer(shadowMapResolution=8192) + while (1): t+=dt p.configureDebugVisualizer(lightPosition=[radius*math.sin(t),radius*math.cos(t),3]) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index cf8afeb1d..a86e3f3c9 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -5874,12 +5874,13 @@ static PyObject* pybullet_configureDebugVisualizer(PyObject* self, PyObject* arg int shadowMapResolution = -1; int shadowMapWorldSize = -1; int physicsClientId = 0; + double remoteSyncTransformInterval = -1; PyObject* pyLightPosition = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"flag", "enable", "lightPosition", "shadowMapResolution", "shadowMapWorldSize", "physicsClientId", NULL}; + static char* kwlist[] = {"flag", "enable", "lightPosition", "shadowMapResolution", "shadowMapWorldSize", "remoteSyncTransformInterval", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iiOiii", kwlist, - &flag, &enable, &pyLightPosition, &shadowMapResolution, &shadowMapWorldSize, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|iiOiidi", kwlist, + &flag, &enable, &pyLightPosition, &shadowMapResolution, &shadowMapWorldSize, &remoteSyncTransformInterval, &physicsClientId)) return NULL; sm = getPhysicsClient(physicsClientId); @@ -5911,6 +5912,10 @@ static PyObject* pybullet_configureDebugVisualizer(PyObject* self, PyObject* arg { b3ConfigureOpenGLVisualizerSetShadowMapWorldSize(commandHandle, shadowMapWorldSize); } + if (remoteSyncTransformInterval >= 0) + { + b3ConfigureOpenGLVisualizerSetRemoteSyncTransformInterval(commandHandle, remoteSyncTransformInterval); + } b3SubmitClientCommandAndWaitStatus(sm, commandHandle); }