From e5ed15c3b2ec47ccb1dadb4b9eefe5b4361942f2 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 18 Nov 2019 10:22:56 -0800 Subject: [PATCH] fix memory issues in btSparseSDF.h (hash function on structure with uninitialized padding, and Reset not called in destructor) expose sparseSdfVoxelSize in PyBullet.setPhysicsEngineParameter don't call deformable wireframe drawing in the wrong thread/place (it can cause crashes) --- examples/SharedMemory/PhysicsClientC_API.cpp | 9 ++++ examples/SharedMemory/PhysicsClientC_API.h | 3 +- .../PhysicsServerCommandProcessor.cpp | 48 +++++++++---------- examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/SharedMemory/SharedMemoryPublic.h | 5 +- examples/pybullet/examples/deformable_ball.py | 27 +++++++++++ .../pybullet/examples/deformable_torus.py | 1 + examples/pybullet/pybullet.c | 10 +++- .../btDeformableMultiBodyDynamicsWorld.cpp | 4 +- src/BulletSoftBody/btSparseSDF.h | 9 +++- 10 files changed, 85 insertions(+), 32 deletions(-) create mode 100644 examples/pybullet/examples/deformable_ball.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0f6f255fb..0284d1d1b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -895,6 +895,15 @@ B3_SHARED_API int b3PhysicsParamSetDefaultFrictionCFM(b3SharedMemoryCommandHandl return 0; } +B3_SHARED_API int b3PhysicsParameterSetSparseSdfVoxelSize(b3SharedMemoryCommandHandle commandHandle, double sparseSdfVoxelSize) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_updateFlags |= SIM_PARAM_UPDATE_SPARSE_SDF; + command->m_physSimParamArgs.m_sparseSdfVoxelSize = sparseSdfVoxelSize; + return 0; +} + B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d2037d02e..ff59cef46 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -357,7 +357,8 @@ extern "C" B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType); B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize); B3_SHARED_API int b3PhysicsParamSetSolverAnalytics(b3SharedMemoryCommandHandle commandHandle, int reportSolverAnalytics); - + B3_SHARED_API int b3PhysicsParameterSetSparseSdfVoxelSize(b3SharedMemoryCommandHandle commandHandle, double sparseSdfVoxelSize); + B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient); B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle, struct b3PhysicsSimulationParameters* params); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2d2a326fa..f5d5673ab 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2605,7 +2605,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformableWorld() { btDeformableMultiBodyDynamicsWorld* world = 0; - if (m_data->m_dynamicsWorld->getWorldType()== BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD) + if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType()== BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD) { world = (btDeformableMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; } @@ -2615,7 +2615,7 @@ btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformable btSoftMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getSoftWorld() { btSoftMultiBodyDynamicsWorld* world = 0; - if (m_data->m_dynamicsWorld->getWorldType()== BT_SOFT_MULTIBODY_DYNAMICS_WORLD) + if (m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getWorldType()== BT_SOFT_MULTIBODY_DYNAMICS_WORLD) { world = (btSoftMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; } @@ -9437,6 +9437,26 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM; } + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_SPARSE_SDF) + { + { + btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); + if (deformWorld) + { + deformWorld ->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize); + deformWorld ->getWorldInfo().m_sparsesdf.Reset(); + } + } + { + btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld(); + if (softWorld) + { + softWorld->getWorldInfo().m_sparsesdf.setDefaultVoxelsz(clientCmd.m_physSimParamArgs.m_sparseSdfVoxelSize); + softWorld->getWorldInfo().m_sparsesdf.Reset(); + } + } + } + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD) { m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold; @@ -12966,30 +12986,6 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags) } m_data->m_guiHelper->render(m_data->m_dynamicsWorld); -#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD -#ifndef SKIP_DEFORMABLE_BODY - if (m_data->m_dynamicsWorld) - { - btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld(); - if (deformWorld) - { - if (deformWorld->getSoftBodyArray().size()) - { - for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++) - { - btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i]; - { - btSoftBodyHelpers::DrawFrame(psb, m_data->m_dynamicsWorld->getDebugDrawer()); - btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), deformWorld->getDrawFlags()); - } - } - m_data->m_guiHelper->drawDebugDrawerLines(); - m_data->m_guiHelper->clearLines(); - } - } - } -#endif -#endif } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 4d4324013..ec79d77c5 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -484,6 +484,7 @@ enum EnumSimParamUpdateFlags SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26, SIM_PARAM_UPDATE_WARM_STARTING_FACTOR = 1 << 27, SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR = 1 << 28, + SIM_PARAM_UPDATE_SPARSE_SDF = 1 << 29, }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 2e3d9ae26..feed391a8 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,7 +7,9 @@ //Please don't replace an existing magic number: //instead, only ADD a new one at the top, comment-out previous one -#define SHARED_MEMORY_MAGIC_NUMBER 201909030 + +#define SHARED_MEMORY_MAGIC_NUMBER 201911180 +//#define SHARED_MEMORY_MAGIC_NUMBER 201909030 //#define SHARED_MEMORY_MAGIC_NUMBER 201908110 //#define SHARED_MEMORY_MAGIC_NUMBER 201908050 //#define SHARED_MEMORY_MAGIC_NUMBER 2019060190 @@ -978,6 +980,7 @@ struct b3PhysicsSimulationParameters int m_constraintSolverType; int m_minimumSolverIslandSize; int m_reportSolverAnalytics; + double m_sparseSdfVoxelSize; }; diff --git a/examples/pybullet/examples/deformable_ball.py b/examples/pybullet/examples/deformable_ball.py new file mode 100644 index 000000000..8803e2fb2 --- /dev/null +++ b/examples/pybullet/examples/deformable_ball.py @@ -0,0 +1,27 @@ +import pybullet as p +from time import sleep + +physicsClient = p.connect(p.GUI) + +p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) + +p.setGravity(0, 0, -10) + +planeOrn = [0,0,0,1]#p.getQuaternionFromEuler([0.3,0,0]) +planeId = p.loadURDF("plane.urdf", [0,0,-2],planeOrn) + +boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) + +ballId = p.loadSoftBody("ball.vtk", basePosition = [0,0,-1], scale = 0.5, mass = 0.1, useNeoHookean = 1, NeoHookeanMu = 20, NeoHookeanLambda = 20, NeoHookeanDamping = 0.001, useSelfCollision = 1, frictionCoeff = .5) +p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) +p.setRealTimeSimulation(1) + +#logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "perf.json") + +while p.isConnected(): + + p.setGravity(0,0,-10) + sleep(1./240.) + +#p.resetSimulation() +#p.stopStateLogging(logId) \ No newline at end of file diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py index ee5c78772..ff02237d6 100644 --- a/examples/pybullet/examples/deformable_torus.py +++ b/examples/pybullet/examples/deformable_torus.py @@ -12,6 +12,7 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2]) boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5) +p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25) p.setRealTimeSimulation(1) while p.isConnected(): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0b1ef28cf..260f6db68 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1583,6 +1583,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar int reportSolverAnalytics = -1; double warmStartingFactor = -1; + double sparseSdfVoxelSize = -1; int physicsClientId = 0; @@ -1611,11 +1612,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar "minimumSolverIslandSize", "reportSolverAnalytics", "warmStartingFactor", + "sparseSdfVoxelSize", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiiddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, - &reportSolverAnalytics, &warmStartingFactor, &physicsClientId)) + &reportSolverAnalytics, &warmStartingFactor, &sparseSdfVoxelSize, &physicsClientId)) { return NULL; } @@ -1740,6 +1742,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetWarmStartingFactor(command, warmStartingFactor); } + if (sparseSdfVoxelSize >= 0) + { + b3PhysicsParameterSetSparseSdfVoxelSize(command, sparseSdfVoxelSize); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } diff --git a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp index f16786e16..25ee65450 100644 --- a/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp +++ b/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp @@ -382,6 +382,8 @@ void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep) void btDeformableMultiBodyDynamicsWorld::debugDrawWorld() { + btMultiBodyDynamicsWorld::debugDrawWorld(); + for (int i = 0; i < getSoftBodyArray().size(); i++) { btSoftBody* psb = (btSoftBody*)getSoftBodyArray()[i]; @@ -391,7 +393,7 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld() } } - btMultiBodyDynamicsWorld::debugDrawWorld(); + } void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep) diff --git a/src/BulletSoftBody/btSparseSDF.h b/src/BulletSoftBody/btSparseSDF.h index 8258c3eeb..37e657d62 100644 --- a/src/BulletSoftBody/btSparseSDF.h +++ b/src/BulletSoftBody/btSparseSDF.h @@ -77,6 +77,10 @@ struct btSparseSdf int nprobes; int nqueries; + ~btSparseSdf() + { + Reset(); + } // // Methods // @@ -330,15 +334,18 @@ struct btSparseSdf { struct btS { - int x, y, z; + int x, y, z, w; void* p; }; btS myset; + //memset may be needed in case of additional (uninitialized) padding! + //memset(myset, 0, sizeof(btS)); myset.x = x; myset.y = y; myset.z = z; + myset.w = 0; myset.p = (void*)shape; const void* ptr = &myset;