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;

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -319,6 +319,8 @@ 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);

3
setup.py
View File

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

3
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
View File

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

20
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -277,6 +277,10 @@ 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)
{
@@ -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()
{

2
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

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