diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d88e15d42..f56f7b67b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -620,6 +620,16 @@ B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle com return 0; } +B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_constraintSolverType = constraintSolverType; + command->m_updateFlags |= SIM_PARAM_CONSTRAINT_SOLVER_TYPE; + return 0; +} + + B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2870f7f1e..fdf846c72 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -319,7 +319,9 @@ B3_SHARED_API int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHa B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold); B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop); B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT); - +B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType); + + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d8d6cd789..1bbe02dc3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8,6 +8,10 @@ #include "BulletCollision/CollisionDispatch/btInternalEdgeUtility.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" + #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" @@ -1599,6 +1603,7 @@ struct PhysicsServerCommandProcessorInternalData btMultiBodyDynamicsWorld* m_dynamicsWorld; #endif + int m_constraintSolverType; SharedMemoryDebugDrawer* m_remoteDebugDrawer; btAlignedObjectArray m_cachedContactPoints; @@ -1657,6 +1662,7 @@ struct PhysicsServerCommandProcessorInternalData m_solver(0), m_collisionConfiguration(0), m_dynamicsWorld(0), + m_constraintSolverType(-1), m_remoteDebugDrawer(0), m_stateLoggersUniqueId(0), m_profileTimingLoggingUid(-1), @@ -2340,6 +2346,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() { + m_data->m_constraintSolverType=eConstraintSolverLCP_SI; ///collision configuration contains default setup for memory, collision setup //m_collisionConfiguration->setConvexConvexMultipointIterations(); #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD @@ -7348,7 +7355,60 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st { m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT!=0; } + if (clientCmd.m_updateFlags&SIM_PARAM_CONSTRAINT_SOLVER_TYPE) + { + //check if the current type is different from requested one + if (m_data->m_constraintSolverType!=clientCmd.m_physSimParamArgs.m_constraintSolverType) + { + m_data->m_constraintSolverType =clientCmd.m_physSimParamArgs.m_constraintSolverType; + + btConstraintSolver* oldSolver = m_data->m_dynamicsWorld->getConstraintSolver(); + + btMultiBodyConstraintSolver* newSolver = 0; + + switch (clientCmd.m_physSimParamArgs.m_constraintSolverType) + { + case eConstraintSolverLCP_SI: + { + newSolver = new btMultiBodyConstraintSolver; + b3Printf("PyBullet: Constraint Solver: btMultiBodyConstraintSolver\n"); + break; + } + case eConstraintSolverLCP_PGS: + { + btSolveProjectedGaussSeidel* mlcp = new btSolveProjectedGaussSeidel(); + newSolver = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("PyBullet: Constraint Solver: MLCP + PGS\n"); + break; + } + case eConstraintSolverLCP_DANTZIG: + { + btDantzigSolver* mlcp = new btDantzigSolver(); + newSolver = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("PyBullet: Constraint Solver: MLCP + Dantzig\n"); + break; + } + case eConstraintSolverLCP_BLOCK_PGS: + { + break; + } + default: + { + + } + }; + if (newSolver) + { + delete oldSolver; + + m_data->m_dynamicsWorld->setMultiBodyConstraintSolver(newSolver); + m_data->m_solver = newSolver; + printf("switched solver\n"); + } + + } + } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index e92333b43..509e2010b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -467,6 +467,7 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 2097152, SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304, SIM_PARAM_ENABLE_SAT = 8388608, + SIM_PARAM_CONSTRAINT_SOLVER_TYPE =16777216, }; enum EnumLoadSoftBodyUpdateFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index cd5251928..9e403ece8 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -897,7 +897,17 @@ struct b3PhysicsSimulationParameters double m_solverResidualThreshold; double m_contactSlop; int m_enableSAT; + int m_constraintSolverType; }; +enum eConstraintSolverTypes +{ + eConstraintSolverLCP_SI=1, + eConstraintSolverLCP_PGS, + eConstraintSolverLCP_DANTZIG, + eConstraintSolverLCP_LEMKE, + eConstraintSolverLCP_NNCG, + eConstraintSolverLCP_BLOCK_PGS, +}; #endif//SHARED_MEMORY_PUBLIC_H diff --git a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp index 6e962470f..21fbc8354 100644 --- a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp +++ b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp @@ -25,8 +25,6 @@ B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* conte { CollisionFilterMyClass* obj = new CollisionFilterMyClass(); context->m_userPointer = obj; - - printf("hi!\n"); return SHARED_MEMORY_MAGIC_NUMBER; } @@ -93,6 +91,4 @@ B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* cont CollisionFilterMyClass* obj = (CollisionFilterMyClass*) context->m_userPointer; delete obj; context->m_userPointer = 0; - - printf("bye!\n"); } diff --git a/examples/pybullet/examples/switchConstraintSolver.py b/examples/pybullet/examples/switchConstraintSolver.py new file mode 100644 index 000000000..3ed0cfb85 --- /dev/null +++ b/examples/pybullet/examples/switchConstraintSolver.py @@ -0,0 +1,31 @@ +import pybullet as p +import time + +p.connect(p.GUI) +#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) +p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_DANTZIG, globalCFM = 0.000001) +#p.setPhysicsEngineParameter(constraintSolverType=p.CONSTRAINT_SOLVER_LCP_PGS, globalCFM = 0.0001) + + +p.loadURDF("plane.urdf") +radius = 0.025 +distance = 1.86 +yaw=135 +pitch=-11 +targetPos=[0,0,0] + +p.setPhysicsEngineParameter(solverResidualThreshold=0.001, numSolverIterations=200) +p.resetDebugVisualizerCamera(distance, yaw,pitch, targetPos) +objectId = -1 + +for i in range (10): + objectId = p.loadURDF("cube_small.urdf",[1,1,radius+i*2*radius]) + +p.changeDynamics(objectId,-1,100) + +timeStep = 1./240. +p.setGravity(0,0,-10) +while (p.isConnected()): + p.stepSimulation() + time.sleep(timeStep) + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ff92f8929..93c802e15 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1466,12 +1466,15 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double solverResidualThreshold = -1; double contactSlop = -1; int enableSAT = -1; + int constraintSolverType=-1; + double globalCFM = -1; + int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "deterministicOverlappingPairs", "allowedCcdPenetration", "jointFeedbackMode", "solverResidualThreshold", "contactSlop", "enableSAT", "constraintSolverType", "globalCFM", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &physicsClientId)) { return NULL; } @@ -1576,6 +1579,14 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParameterSetEnableSAT(command, enableSAT); } + if (constraintSolverType>=0) + { + b3PhysicsParameterSetConstraintSolverType(command, constraintSolverType); + } + if (globalCFM>=0) + { + b3PhysicsParamSetDefaultGlobalCFM(command, globalCFM); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } @@ -9639,6 +9650,13 @@ initpybullet(void) PyModule_AddIntConstant(m, "CONTACT_REPORT_EXISTING", CONTACT_QUERY_MODE_REPORT_EXISTING_CONTACT_POINTS); PyModule_AddIntConstant(m, "CONTACT_RECOMPUTE_CLOSEST", CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS); + PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_SI",eConstraintSolverLCP_SI); + PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_PGS",eConstraintSolverLCP_PGS); + PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_DANTZIG",eConstraintSolverLCP_DANTZIG); + //PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_LEMKE",eConstraintSolverLCP_LEMKE); + //PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_NNCF",eConstraintSolverLCP_NNCG); + //PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_BLOCK",eConstraintSolverLCP_BLOCK_PGS); + PyModule_AddIntConstant(m, "VR_BUTTON_IS_DOWN", eButtonIsDown); PyModule_AddIntConstant(m, "VR_BUTTON_WAS_TRIGGERED", eButtonTriggered); PyModule_AddIntConstant(m, "VR_BUTTON_WAS_RELEASED", eButtonReleased); diff --git a/setup.py b/setup.py index a660aeb04..600570d35 100644 --- a/setup.py +++ b/setup.py @@ -261,6 +261,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBody.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp"]\ ++["src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"]\ @@ -453,7 +454,7 @@ print("-----") setup( name = 'pybullet', - version='2.1.2', + version='2.1.3', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', diff --git a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h index 890afe6da..0491639f7 100644 --- a/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btConstraintSolver.h @@ -34,7 +34,8 @@ enum btConstraintSolverType { BT_SEQUENTIAL_IMPULSE_SOLVER=1, BT_MLCP_SOLVER=2, - BT_NNCG_SOLVER=4 + BT_NNCG_SOLVER=4, + BT_MULTIBODY_SOLVER=8, }; class btConstraintSolver diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index a2be068ed..da6cbe4b7 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -277,7 +277,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: m_multiBodyConstraints.resize(0); } - + void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) + { + m_solver = solver; + } + virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) { if (islandId<0) @@ -394,6 +398,22 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () delete m_solverMultiBodyIslandCallback; } +void btMultiBodyDynamicsWorld::setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver) +{ + m_multiBodyConstraintSolver = solver; + m_solverMultiBodyIslandCallback->setMultiBodyConstraintSolver(solver); + btDiscreteDynamicsWorld::setConstraintSolver(solver); +} + +void btMultiBodyDynamicsWorld::setConstraintSolver(btConstraintSolver* solver) +{ + if (solver->getSolverType()==BT_MULTIBODY_SOLVER) + { + m_multiBodyConstraintSolver = (btMultiBodyConstraintSolver*)solver; + } + btDiscreteDynamicsWorld::setConstraintSolver(solver); +} + void btMultiBodyDynamicsWorld::forwardKinematics() { @@ -1008,4 +1028,4 @@ void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) } } -} \ No newline at end of file +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index c0c132bbb..2fbf089d8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -109,6 +109,8 @@ public: virtual void applyGravity(); virtual void serialize(btSerializer* serializer); + virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver); + virtual void setConstraintSolver(btConstraintSolver* solver); }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H