expose choice of contraint solver in PyBullet, with switchConstraintSolver example

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;

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);
+    
+    
 
 
 

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<b3ContactPointData> 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)
 	{

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

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

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");
 }

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)
+

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,
+	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, &physicsClientId))
+									 &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);

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',

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

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)
 		}
 	}
 
-}
+}

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