Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

expose choice of contraint solver in PyBullet, with switchConstraintSolver example

Browse Source
This commit is contained in:
Erwin Coumans
2018-08-23 23:04:17 -07:00
parent d44571a4b3
commit 61b7591b8e
12 changed files with 164 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;

4
examples/SharedMemory/PhysicsClientC_API.h
View File

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

60
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

10
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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

4
examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp
View File

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

31
examples/pybullet/examples/switchConstraintSolver.py Normal file
View File

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

24
examples/pybullet/pybullet.c
View File

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